diff --git a/.ackrc b/.ackrc deleted file mode 100644 index 0215ca1eebeb..000000000000 --- a/.ackrc +++ /dev/null @@ -1 +0,0 @@ ---ignore-dir=Documentation diff --git a/.ci/Jenkinsfile-compile b/.ci/Jenkinsfile-compile index c1907fa64c58..04c8c210b94a 100644 --- a/.ci/Jenkinsfile-compile +++ b/.ci/Jenkinsfile-compile @@ -40,16 +40,16 @@ 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_cannode_canbootloader", + "ark_cannode_default", "ark_fmu-v6x_bootloader", "ark_fmu-v6x_default", "atl_mantis-edu_default", "av_x-v1_default", - "bitcraze_crazyflie_default", "bitcraze_crazyflie21_default", + "bitcraze_crazyflie_default", "cuav_can-gps-v1_canbootloader", "cuav_can-gps-v1_default", "cuav_nora_default", @@ -66,8 +66,8 @@ pipeline { "holybro_durandal-v1_default", "holybro_kakutef7_default", "holybro_kakuteh7_default", - "holybro_kakuteh7v2_default", "holybro_kakuteh7mini_default", + "holybro_kakuteh7v2_default", "holybro_pix32v5_default", "matek_gnss-m9n-f4_canbootloader", "matek_gnss-m9n-f4_default", @@ -77,8 +77,8 @@ pipeline { "modalai_fc-v1_default", "modalai_fc-v2_default", "mro_ctrl-zero-classic_default", - "mro_ctrl-zero-f7_default", "mro_ctrl-zero-f7-oem_default", + "mro_ctrl-zero-f7_default", "mro_ctrl-zero-h7-oem_default", "mro_ctrl-zero-h7_default", "mro_pixracerpro_default", @@ -104,22 +104,28 @@ pipeline { "px4_fmu-v5_debug", "px4_fmu-v5_default", "px4_fmu-v5_lto", + "px4_fmu-v5_rover", "px4_fmu-v5_stackcheck", "px4_fmu-v5_uavcanv0periph", "px4_fmu-v5x_default", + "px4_fmu-v5x_rover", "px4_fmu-v6c_default", + "px4_fmu-v6c_rover", "px4_fmu-v6u_default", + "px4_fmu-v6u_rover", "px4_fmu-v6x_default", + "px4_fmu-v6x_rover", "px4_fmu-v6xrt_bootloader", "px4_fmu-v6xrt_default", + "px4_fmu-v6xrt_rover", "px4_io-v2_default", "raspberrypi_pico_default", + "siyi_n7_default", "sky-drones_smartap-airlink_default", "spracing_h7extreme_default", "thepeach_k1_default", "thepeach_r1_default", "uvify_core_default", - "siyi_n7_default" ], image: docker_images.nuttx, archive: true diff --git a/.editorconfig b/.editorconfig new file mode 100644 index 000000000000..87496c115e9e --- /dev/null +++ b/.editorconfig @@ -0,0 +1,6 @@ +root = true + +[*.{c,cpp,cc,h,hpp}] +indent_style = tab +tab_width = 8 +max_line_length = 120 diff --git a/.github/workflows/compile_nuttx.yml b/.github/workflows/compile_nuttx.yml index f2ffbc745f3e..cf42be021321 100644 --- a/.github/workflows/compile_nuttx.yml +++ b/.github/workflows/compile_nuttx.yml @@ -25,6 +25,7 @@ jobs: ark_can-rtk-gps, ark_cannode, ark_fmu-v6x, + ark_septentrio-gps, atl_mantis-edu, av_x-v1, bitcraze_crazyflie, @@ -48,6 +49,7 @@ jobs: matek_h743-slim, modalai_fc-v1, modalai_fc-v2, + mro_ctrl-zero-classic, mro_ctrl-zero-f7, mro_ctrl-zero-f7-oem, mro_ctrl-zero-h7, diff --git a/.github_changelog_generator b/.github_changelog_generator deleted file mode 100644 index b89c58ad0567..000000000000 --- a/.github_changelog_generator +++ /dev/null @@ -1,33 +0,0 @@ -# How to install: -# gem install github_changelog_generator -# How to run: -# github_changelog_generator -u PX4 -p Firmware -# Description: -# The following params are sensible defaults for the PX4 project, -# if you want to do a changelog before a release you need to update since-tag and future-releases, - -# Params: -# github_changelog_generator --help for all options - -# max-issues -# max threshold for github api queries -# make sure you set your CHANGELOG_GITHUB_TOKEN before -# running -max-issues=1500 - -# exclude-tags-regex -# excludes release candidates -exclude-tags-regex=rc[0-9]{1,}|beta[0-9]{1,} - -# since-tag -# version of last stable release -# you need to change this depending on what you need -# if you want a changelog between versions this is the lowest version -since-tag=1.6.5 - -# future-release -# version you are about to release -# if you want a changelog between a version and all unreleased changes grouped as a release -# eg: v1.6.5 to v1.7.0 -future-release=v1.7.0 - diff --git a/.gitmodules b/.gitmodules index 62b18dd75ce8..e443da19a290 100644 --- a/.gitmodules +++ b/.gitmodules @@ -79,3 +79,7 @@ [submodule "Tools/simulation/gz"] path = Tools/simulation/gz url = https://github.com/PX4/PX4-gazebo-models.git + branch = main +[submodule "boards/modalai/voxl2/libfc-sensor-api"] + path = boards/modalai/voxl2/libfc-sensor-api + url = https://gitlab.com/voxl-public/voxl-sdk/core-libs/libfc-sensor-api.git diff --git a/.travis.yml b/.travis.yml deleted file mode 100644 index 347ccc884c89..000000000000 --- a/.travis.yml +++ /dev/null @@ -1,35 +0,0 @@ -language: cpp - -git: - depth: 100 - submodules: false - -matrix: - fast_finish: true - include: - - os: linux - dist: xenial - # In order to stay under the coverity rate limit, we only run this weekly - # and not on push which is configured in travis-ci settings. - if: branch = main - -before_install: - - echo -n | openssl s_client -connect scan.coverity.com:443 | sed -ne '/-BEGIN CERTIFICATE-/,/-END CERTIFICATE-/p' | sudo tee -a /etc/ssl/certs/ca- - -install: - - export PATH=$HOME/.local/bin:$PATH - - pip install --user --upgrade pip - - pip install --user -r Tools/setup/requirements.txt - -script: - - make - -addons: - coverity_scan: - project: - name: "PX4/Firmware" - description: "Build submitted via Travis CI" - notification_email: ci@px4.io - build_command_prepend: "make distclean" - build_command: "make px4_sitl_default" - branch_pattern: coverity_scan diff --git a/.vscode/cmake-variants.yaml b/.vscode/cmake-variants.yaml index c929a3054490..c2a057d68669 100644 --- a/.vscode/cmake-variants.yaml +++ b/.vscode/cmake-variants.yaml @@ -71,6 +71,26 @@ CONFIG: buildType: MinSizeRel settings: CONFIG: px4_fmu-v5x_default + px4_fmu-v6c_default: + short: px4_fmu-v6c + buildType: MinSizeRel + settings: + CONFIG: px4_fmu-v6c_default + px4_fmu-v6c_bootloader: + short: px4_fmu-v6c_bootloader + buildType: MinSizeRel + settings: + CONFIG: px4_fmu-v6c_bootloader + px4_fmu-v6u_default: + short: px4_fmu-v6u + buildType: MinSizeRel + settings: + CONFIG: px4_fmu-v6u_default + px4_fmu-v6u_bootloader: + short: px4_fmu-v6u_bootloader + buildType: MinSizeRel + settings: + CONFIG: px4_fmu-v6u_bootloader px4_fmu-v6x_default: short: px4_fmu-v6x buildType: MinSizeRel @@ -131,6 +151,16 @@ CONFIG: buildType: MinSizeRel settings: CONFIG: ark_can-rtk-gps_canbootloader + ark_septentrio_gps_default: + short: ark_septentrio_gps_default + buildType: MinSizeRel + settings: + CONFIG: ark_septentrio_gps_default + ark_septentrio_gps_canbootloader: + short: ark_septentrio_gps_canbootloader + buildType: MinSizeRel + settings: + CONFIG: ark_septentrio_gps_canbootloader ark_cannode_default: short: ark_cannode_default buildType: MinSizeRel diff --git a/.ycm_extra_conf.py b/.ycm_extra_conf.py deleted file mode 100644 index 4f7cfc202001..000000000000 --- a/.ycm_extra_conf.py +++ /dev/null @@ -1,172 +0,0 @@ -# This file is NOT licensed under the GPLv3, which is the license for the rest -# of YouCompleteMe. -# -# Here's the license text for this file: -# -# This is free and unencumbered software released into the public domain. -# -# Anyone is free to copy, modify, publish, use, compile, sell, or -# distribute this software, either in source code form or as a compiled -# binary, for any purpose, commercial or non-commercial, and by any -# means. -# -# In jurisdictions that recognize copyright laws, the author or authors -# of this software dedicate any and all copyright interest in the -# software to the public domain. We make this dedication for the benefit -# of the public at large and to the detriment of our heirs and -# successors. We intend this dedication to be an overt act of -# relinquishment in perpetuity of all present and future rights to this -# software under copyright law. -# -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, -# EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF -# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. -# IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR -# OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, -# ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR -# OTHER DEALINGS IN THE SOFTWARE. -# -# For more information, please refer to - -import os -import ycm_core - -# These are the compilation flags that will be used in case there's no -# compilation database set (by default, one is not set). -# CHANGE THIS LIST OF FLAGS. YES, THIS IS THE DROID YOU HAVE BEEN LOOKING FOR. -flags = [ -'-Wall', -'-Wextra', -'-Werror', -#'-Wc++98-compat', -'-Wno-long-long', -'-Wno-variadic-macros', -'-fexceptions', -'-DNDEBUG', -# You 100% do NOT need -DUSE_CLANG_COMPLETER in your flags; only the YCM -# source code needs it. -#'-DUSE_CLANG_COMPLETER', -# THIS IS IMPORTANT! Without a "-std=" flag, clang won't know which -# language to use when compiling headers. So it will guess. Badly. So C++ -# headers will be compiled as C headers. You don't want that so ALWAYS specify -# a "-std=". -# For a C project, you would set this to something like 'c99' instead of -# 'c++14'. -'-std=c++14', -# ...and the same thing goes for the magic -x option which specifies the -# language that the files to be compiled are written in. This is mostly -# relevant for c++ headers. -# For a C project, you would set this to 'c' instead of 'c++'. -'-x', -'c++', -'-undef', # get rid of standard definitions to allow us to include arm math header -'-I', os.path.join(os.path.expanduser("~"),'gcc-arm-none-eabi-4_7-2013q3/arm-none-eabi/include'), -'-I', 'Build/px4_io-v2_default.build/nuttx-export/include/', -'-I', './NuttX/nuttx/arch/arm/include', -'-include', './src/include/visibility.h', -'-I', './src', -'-I', './src/modules', -'-I', './src/include', -'-I', './src/lib', -'-I', './NuttX', -] - - -# Set this to the absolute path to the folder (NOT the file!) containing the -# compile_commands.json file to use that instead of 'flags'. See here for -# more details: http://clang.llvm.org/docs/JSONCompilationDatabase.html -# -# Most projects will NOT need to set this to anything; you can just change the -# 'flags' list of compilation flags. Notice that YCM itself uses that approach. -compilation_database_folder = '' - -if os.path.exists( compilation_database_folder ): - database = ycm_core.CompilationDatabase( compilation_database_folder ) -else: - database = None - -SOURCE_EXTENSIONS = [ '.cpp', '.cxx', '.cc', '.c', '.m', '.mm' ] - -def DirectoryOfThisScript(): - return os.path.dirname( os.path.abspath( __file__ ) ) - - -def MakeRelativePathsInFlagsAbsolute( flags, working_directory ): - if not working_directory: - return list( flags ) - new_flags = [] - make_next_absolute = False - path_flags = [ '-isystem', '-I', '-iquote', '--sysroot=' ] - for flag in flags: - new_flag = flag - - if make_next_absolute: - make_next_absolute = False - if not flag.startswith( '/' ): - new_flag = os.path.join( working_directory, flag ) - - for path_flag in path_flags: - if flag == path_flag: - make_next_absolute = True - break - - if flag.startswith( path_flag ): - path = flag[ len( path_flag ): ] - new_flag = path_flag + os.path.join( working_directory, path ) - break - - if new_flag: - new_flags.append( new_flag ) - return new_flags - - -def IsHeaderFile( filename ): - extension = os.path.splitext( filename )[ 1 ] - return extension in [ '.h', '.hxx', '.hpp', '.hh' ] - - -def GetCompilationInfoForFile( filename ): - # The compilation_commands.json file generated by CMake does not have entries - # for header files. So we do our best by asking the db for flags for a - # corresponding source file, if any. If one exists, the flags for that file - # should be good enough. - if IsHeaderFile( filename ): - basename = os.path.splitext( filename )[ 0 ] - for extension in SOURCE_EXTENSIONS: - replacement_file = basename + extension - if os.path.exists( replacement_file ): - compilation_info = database.GetCompilationInfoForFile( - replacement_file ) - if compilation_info.compiler_flags_: - return compilation_info - return None - return database.GetCompilationInfoForFile( filename ) - - -def FlagsForFile( filename, **kwargs ): - if database: - # Bear in mind that compilation_info.compiler_flags_ does NOT return a - # python list, but a "list-like" StringVec object - compilation_info = GetCompilationInfoForFile( filename ) - if not compilation_info: - return None - - final_flags = MakeRelativePathsInFlagsAbsolute( - compilation_info.compiler_flags_, - compilation_info.compiler_working_dir_ ) - - # NOTE: This is just for YouCompleteMe; it's highly likely that your project - # does NOT need to remove the stdlib flag. DO NOT USE THIS IN YOUR - # ycm_extra_conf IF YOU'RE NOT 100% SURE YOU NEED IT. - #try: - # final_flags.remove( '-stdlib=libc++' ) - #except ValueError: - # pass - else: - relative_to = DirectoryOfThisScript() - final_flags = MakeRelativePathsInFlagsAbsolute( flags, relative_to ) - - return { - 'flags': final_flags, - 'do_cache': True - } diff --git a/Firmware.sublime-project b/Firmware.sublime-project deleted file mode 100644 index d89a58de3f19..000000000000 --- a/Firmware.sublime-project +++ /dev/null @@ -1,83 +0,0 @@ -{ - "folders": - [ - { - "path": ".", - "file_exclude_patterns": - [ - "*.o", - "*.a", - "*.d", - ".built", - ".context", - ".depend", - ".config", - ".version", - "Make.dep", - ".configured", - "*.sublime-project", - "*.sublime-workspace", - ".project", - ".cproject", - "cscope.out" - ], - "folder_exclude_patterns": - [ - ".settings", - "nuttx/arch/arm/src/board", - "nuttx/arch/arm/src/chip", - "build_*" - ] - } - ], - "settings": - { - "tab_size": 8, - "translate_tabs_to_spaces": false, - "highlight_line": true, - "AStyleFormatter": - { - "options_c": - { - "use_only_additional_options": true, - "additional_options_file": "${project_path}/Tools/astyle/astylerc" - }, - "options_c++": - { - "use_only_additional_options": true, - "additional_options_file": "${project_path}/Tools/astyle/astylerc" - } - } - }, - "build_systems": - [ - { - "name": "PX4: make all", - "working_dir": "${project_path}", - "file_regex": "^(..[^:]*):([0-9]+):?([0-9]+)?:? (.*)$", - "cmd": ["make"], - "shell": true - }, - { - "name": "PX4: make and upload", - "working_dir": "${project_path}", - "file_regex": "^(..[^:]*):([0-9]+):?([0-9]+)?:? (.*)$", - "cmd": ["make upload px4_fmu-v2_default -j8"], - "shell": true - }, - { - "name": "PX4: make posix", - "working_dir": "${project_path}", - "file_regex": "^(..[^:]*):([0-9]+):?([0-9]+)?:? (.*)$", - "cmd": ["make posix"], - "shell": true - }, - { - "name": "MindPX_V2: make and upload", - "working_dir": "${project_path}", - "file_regex": "^(..[^:]*):([0-9]+):?([0-9]+)?:? (.*)$", - "cmd": ["make upload mindpx-v2_default -j8"], - "shell": true - } - ] -} diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10040_sihsim_quadx b/ROMFS/px4fmu_common/init.d-posix/airframes/10040_sihsim_quadx index 3124ff4f69d5..0849dc91d6f4 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10040_sihsim_quadx +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10040_sihsim_quadx @@ -22,18 +22,17 @@ param set-default CBRK_SUPPLY_CHK 894281 # - without safety switch param set-default CBRK_IO_SAFETY 22027 +# Square quadrotor X PX4 numbering param set-default CA_ROTOR_COUNT 4 -param set-default CA_ROTOR0_PX 0.15 -param set-default CA_ROTOR0_PY 0.15 -param set-default CA_ROTOR0_KM 0.05 -param set-default CA_ROTOR1_PX -0.15 -param set-default CA_ROTOR1_PY -0.15 -param set-default CA_ROTOR1_KM 0.05 -param set-default CA_ROTOR2_PX 0.15 -param set-default CA_ROTOR2_PY -0.15 +param set-default CA_ROTOR0_PX 1 +param set-default CA_ROTOR0_PY 1 +param set-default CA_ROTOR1_PX -1 +param set-default CA_ROTOR1_PY -1 +param set-default CA_ROTOR2_PX 1 +param set-default CA_ROTOR2_PY -1 param set-default CA_ROTOR2_KM -0.05 -param set-default CA_ROTOR3_PX -0.15 -param set-default CA_ROTOR3_PY 0.15 +param set-default CA_ROTOR3_PX -1 +param set-default CA_ROTOR3_PY 1 param set-default CA_ROTOR3_KM -0.05 param set-default PWM_MAIN_FUNC1 101 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10041_sihsim_airplane b/ROMFS/px4fmu_common/init.d-posix/airframes/10041_sihsim_airplane index 47f8c70b00e7..8580464c4906 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10041_sihsim_airplane +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10041_sihsim_airplane @@ -26,7 +26,7 @@ param set-default CBRK_IO_SAFETY 22027 param set-default BAT_N_CELLS 3 -param set-default SIH_T_MAX 6.0 +param set-default SIH_T_MAX 6 param set-default SIH_MASS 0.3 param set-default SIH_IXX 0.00402 param set-default SIH_IYY 0.0144 @@ -45,9 +45,9 @@ param set-default CA_ROTOR_COUNT 1 param set-default CA_SV_CS_COUNT 3 param set-default CA_SV_CS0_TRQ_R -0.5 param set-default CA_SV_CS0_TYPE 1 -param set-default CA_SV_CS1_TRQ_P 1.0 +param set-default CA_SV_CS1_TRQ_P 1 param set-default CA_SV_CS1_TYPE 3 -param set-default CA_SV_CS2_TRQ_Y 1.0 +param set-default CA_SV_CS2_TRQ_Y 1 param set-default CA_SV_CS2_TYPE 4 param set-default PWM_MAIN_FUNC3 201 param set-default PWM_MAIN_FUNC4 202 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert b/ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert index 278281787361..536fc6076c39 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert @@ -35,14 +35,14 @@ param set-default CBRK_IO_SAFETY 22027 param set-default BAT_N_CELLS 3 -param set-default SIH_T_MAX 2.0 +param set-default SIH_T_MAX 2 param set-default SIH_Q_MAX 0.0165 param set-default SIH_MASS 0.2 # IXX and IZZ are inverted from the thesis as the body frame is pitched by 90 deg param set-default SIH_IXX 0.00354 param set-default SIH_IYY 0.000625 param set-default SIH_IZZ 0.00300 -param set-default SIH_IXZ 0.0 +param set-default SIH_IXZ 0 param set-default SIH_KDV 0.2 param set-default SIH_L_ROLL 0.145 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 28c3e34f7132..38d9019adc70 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1010_gazebo-classic_iris_opt_flow +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1010_gazebo-classic_iris_opt_flow @@ -47,5 +47,5 @@ param set-default MPC_ALT_MODE 2 param set-default SENS_FLOW_ROT 6 param set-default SENS_FLOW_MINHGT 0.7 -param set-default SENS_FLOW_MAXHGT 3.0 +param set-default SENS_FLOW_MAXHGT 3 param set-default SENS_FLOW_MAXR 2.5 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1021_gazebo-classic_uuv_hippocampus b/ROMFS/px4fmu_common/init.d-posix/airframes/1021_gazebo-classic_uuv_hippocampus index ca67af3f0187..f6f07d700ef7 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1021_gazebo-classic_uuv_hippocampus +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1021_gazebo-classic_uuv_hippocampus @@ -8,34 +8,34 @@ param set-default CA_AIRFRAME 7 param set-default CA_ROTOR_COUNT 4 param set-default CA_R_REV 255 -param set-default CA_ROTOR0_AX 1.0000 -param set-default CA_ROTOR0_AY 0.0000 -param set-default CA_ROTOR0_AZ 0.0000 -param set-default CA_ROTOR0_KM 0.0000 -param set-default CA_ROTOR0_PX 0.0000 -param set-default CA_ROTOR0_PY -0.3000 -param set-default CA_ROTOR0_PZ -0.3000 -param set-default CA_ROTOR1_AX 1.0000 -param set-default CA_ROTOR1_AY 0.0000 -param set-default CA_ROTOR1_AZ 0.0000 -param set-default CA_ROTOR1_KM 0.0000 -param set-default CA_ROTOR1_PX 0.0000 -param set-default CA_ROTOR1_PY 0.3000 -param set-default CA_ROTOR1_PZ -0.3000 -param set-default CA_ROTOR2_AX 1.0000 -param set-default CA_ROTOR2_AY 0.0000 -param set-default CA_ROTOR2_AZ 0.0000 -param set-default CA_ROTOR2_KM 0.0000 -param set-default CA_ROTOR2_PX 0.0000 -param set-default CA_ROTOR2_PY 0.3000 -param set-default CA_ROTOR2_PZ 0.3000 -param set-default CA_ROTOR3_AX 1.0000 -param set-default CA_ROTOR3_AY 0.0000 -param set-default CA_ROTOR3_AZ 0.0000 -param set-default CA_ROTOR3_KM 0.0000 -param set-default CA_ROTOR3_PX 0.0000 -param set-default CA_ROTOR3_PY -0.3000 -param set-default CA_ROTOR3_PZ 0.3000 +param set-default CA_ROTOR0_AX 1 +param set-default CA_ROTOR0_AY 0 +param set-default CA_ROTOR0_AZ 0 +param set-default CA_ROTOR0_KM 0 +param set-default CA_ROTOR0_PX 0 +param set-default CA_ROTOR0_PY -0.3 +param set-default CA_ROTOR0_PZ -0.3 +param set-default CA_ROTOR1_AX 1 +param set-default CA_ROTOR1_AY 0 +param set-default CA_ROTOR1_AZ 0 +param set-default CA_ROTOR1_KM 0 +param set-default CA_ROTOR1_PX 0 +param set-default CA_ROTOR1_PY 0.3 +param set-default CA_ROTOR1_PZ -0.3 +param set-default CA_ROTOR2_AX 1 +param set-default CA_ROTOR2_AY 0 +param set-default CA_ROTOR2_AZ 0 +param set-default CA_ROTOR2_KM 0 +param set-default CA_ROTOR2_PX 0 +param set-default CA_ROTOR2_PY 0.3 +param set-default CA_ROTOR2_PZ 0.3 +param set-default CA_ROTOR3_AX 1 +param set-default CA_ROTOR3_AY 0 +param set-default CA_ROTOR3_AZ 0 +param set-default CA_ROTOR3_KM 0 +param set-default CA_ROTOR3_PX 0 +param set-default CA_ROTOR3_PY -0.3 +param set-default CA_ROTOR3_PZ 0.3 param set-default PWM_MAIN_FUNC1 101 param set-default PWM_MAIN_FUNC2 102 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1022_gazebo-classic_uuv_bluerov2_heavy b/ROMFS/px4fmu_common/init.d-posix/airframes/1022_gazebo-classic_uuv_bluerov2_heavy index 71d0574e1df2..0c6c802f8d21 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1022_gazebo-classic_uuv_bluerov2_heavy +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1022_gazebo-classic_uuv_bluerov2_heavy @@ -9,49 +9,49 @@ param set-default CA_AIRFRAME 7 param set-default CA_ROTOR_COUNT 8 param set-default CA_R_REV 255 -param set-default CA_ROTOR0_AX 1.0000 -param set-default CA_ROTOR0_AY -1.0000 -param set-default CA_ROTOR0_AZ 0.0000 -param set-default CA_ROTOR0_KM 0.0000 -param set-default CA_ROTOR0_PX 0.5000 -param set-default CA_ROTOR0_PY 0.3000 -param set-default CA_ROTOR0_PZ 0.2000 -param set-default CA_ROTOR1_AX 1.0000 -param set-default CA_ROTOR1_AY 1.0000 -param set-default CA_ROTOR1_AZ 0.0000 -param set-default CA_ROTOR1_KM 0.0000 -param set-default CA_ROTOR1_PX 0.5000 -param set-default CA_ROTOR1_PY -0.3000 -param set-default CA_ROTOR1_PZ 0.2000 -param set-default CA_ROTOR2_AX 1.0000 -param set-default CA_ROTOR2_AY 1.0000 -param set-default CA_ROTOR2_AZ 0.0000 -param set-default CA_ROTOR2_KM 0.0000 -param set-default CA_ROTOR2_PX -0.5000 -param set-default CA_ROTOR2_PY 0.3000 -param set-default CA_ROTOR2_PZ 0.2000 -param set-default CA_ROTOR3_AX 1.0000 -param set-default CA_ROTOR3_AY -1.0000 -param set-default CA_ROTOR3_AZ 0.0000 -param set-default CA_ROTOR3_KM 0.0000 -param set-default CA_ROTOR3_PX -0.5000 -param set-default CA_ROTOR3_PY -0.3000 -param set-default CA_ROTOR3_PZ 0.2000 -param set-default CA_ROTOR4_AZ -1.0000 -param set-default CA_ROTOR4_KM 0.0000 -param set-default CA_ROTOR4_PX 0.5000 -param set-default CA_ROTOR4_PY 0.5000 -param set-default CA_ROTOR5_AZ 1.0000 -param set-default CA_ROTOR5_KM 0.0000 -param set-default CA_ROTOR5_PX 0.5000 -param set-default CA_ROTOR5_PY -0.5000 -param set-default CA_ROTOR6_AZ 1.0000 -param set-default CA_ROTOR6_KM 0.0000 -param set-default CA_ROTOR6_PX -0.5000 -param set-default CA_ROTOR6_PY 0.5000 -param set-default CA_ROTOR7_KM 0.0000 -param set-default CA_ROTOR7_PX -0.5000 -param set-default CA_ROTOR7_PY -0.5000 +param set-default CA_ROTOR0_AX 1 +param set-default CA_ROTOR0_AY -1 +param set-default CA_ROTOR0_AZ 0 +param set-default CA_ROTOR0_KM 0 +param set-default CA_ROTOR0_PX 0.5 +param set-default CA_ROTOR0_PY 0.3 +param set-default CA_ROTOR0_PZ 0.2 +param set-default CA_ROTOR1_AX 1 +param set-default CA_ROTOR1_AY 1 +param set-default CA_ROTOR1_AZ 0 +param set-default CA_ROTOR1_KM 0 +param set-default CA_ROTOR1_PX 0.5 +param set-default CA_ROTOR1_PY -0.3 +param set-default CA_ROTOR1_PZ 0.2 +param set-default CA_ROTOR2_AX 1 +param set-default CA_ROTOR2_AY 1 +param set-default CA_ROTOR2_AZ 0 +param set-default CA_ROTOR2_KM 0 +param set-default CA_ROTOR2_PX -0.5 +param set-default CA_ROTOR2_PY 0.3 +param set-default CA_ROTOR2_PZ 0.2 +param set-default CA_ROTOR3_AX 1 +param set-default CA_ROTOR3_AY -1 +param set-default CA_ROTOR3_AZ 0 +param set-default CA_ROTOR3_KM 0 +param set-default CA_ROTOR3_PX -0.5 +param set-default CA_ROTOR3_PY -0.3 +param set-default CA_ROTOR3_PZ 0.2 +param set-default CA_ROTOR4_AZ -1 +param set-default CA_ROTOR4_KM 0 +param set-default CA_ROTOR4_PX 0.5 +param set-default CA_ROTOR4_PY 0.5 +param set-default CA_ROTOR5_AZ 1 +param set-default CA_ROTOR5_KM 0 +param set-default CA_ROTOR5_PX 0.5 +param set-default CA_ROTOR5_PY -0.5 +param set-default CA_ROTOR6_AZ 1 +param set-default CA_ROTOR6_KM 0 +param set-default CA_ROTOR6_PX -0.5 +param set-default CA_ROTOR6_PY 0.5 +param set-default CA_ROTOR7_KM 0 +param set-default CA_ROTOR7_PX -0.5 +param set-default CA_ROTOR7_PY -0.5 param set-default PWM_MAIN_FUNC1 101 param set-default PWM_MAIN_FUNC2 102 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1030_gazebo-classic_plane b/ROMFS/px4fmu_common/init.d-posix/airframes/1030_gazebo-classic_plane index 462cfe50100a..f203ee14cc48 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1030_gazebo-classic_plane +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1030_gazebo-classic_plane @@ -54,9 +54,9 @@ param set-default CA_SV_CS0_TRQ_R -0.5 param set-default CA_SV_CS0_TYPE 1 param set-default CA_SV_CS1_TRQ_R 0.5 param set-default CA_SV_CS1_TYPE 2 -param set-default CA_SV_CS2_TRQ_P 1.0 +param set-default CA_SV_CS2_TRQ_P 1 param set-default CA_SV_CS2_TYPE 3 -param set-default CA_SV_CS3_TRQ_Y 1.0 +param set-default CA_SV_CS3_TRQ_Y 1 param set-default CA_SV_CS3_TYPE 4 param set-default CA_SV_CS4_TYPE 9 param set-default CA_SV_CS5_TYPE 10 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1031_gazebo-classic_plane_cam b/ROMFS/px4fmu_common/init.d-posix/airframes/1031_gazebo-classic_plane_cam index 743698450c56..3dc3c9ee1a54 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1031_gazebo-classic_plane_cam +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1031_gazebo-classic_plane_cam @@ -57,9 +57,9 @@ param set-default CA_SV_CS0_TRQ_R -0.5 param set-default CA_SV_CS0_TYPE 1 param set-default CA_SV_CS1_TRQ_R 0.5 param set-default CA_SV_CS1_TYPE 2 -param set-default CA_SV_CS2_TRQ_P 1.0 +param set-default CA_SV_CS2_TRQ_P 1 param set-default CA_SV_CS2_TYPE 3 -param set-default CA_SV_CS3_TRQ_Y 1.0 +param set-default CA_SV_CS3_TRQ_Y 1 param set-default CA_SV_CS3_TYPE 4 param set-default CA_SV_CS4_TYPE 9 param set-default CA_SV_CS5_TYPE 10 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 9ce6cb61ddfd..f8fd54636649 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 @@ -57,9 +57,9 @@ param set-default CA_SV_CS0_TRQ_R -0.5 param set-default CA_SV_CS0_TYPE 1 param set-default CA_SV_CS1_TRQ_R 0.5 param set-default CA_SV_CS1_TYPE 2 -param set-default CA_SV_CS2_TRQ_P 1.0 +param set-default CA_SV_CS2_TRQ_P 1 param set-default CA_SV_CS2_TYPE 3 -param set-default CA_SV_CS3_TRQ_Y 1.0 +param set-default CA_SV_CS3_TRQ_Y 1 param set-default CA_SV_CS3_TYPE 4 param set-default CA_SV_CS4_TYPE 9 param set-default CA_SV_CS5_TYPE 10 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1033_jsbsim_rascal b/ROMFS/px4fmu_common/init.d-posix/airframes/1033_jsbsim_rascal index 89675aed623d..06a845be30a3 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1033_jsbsim_rascal +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1033_jsbsim_rascal @@ -42,9 +42,9 @@ param set-default CA_SV_CS0_TRQ_R -0.5 param set-default CA_SV_CS0_TYPE 1 param set-default CA_SV_CS1_TRQ_R 0.5 param set-default CA_SV_CS1_TYPE 2 -param set-default CA_SV_CS2_TRQ_P 1.0 +param set-default CA_SV_CS2_TRQ_P 1 param set-default CA_SV_CS2_TYPE 3 -param set-default CA_SV_CS3_TRQ_Y 1.0 +param set-default CA_SV_CS3_TRQ_Y 1 param set-default CA_SV_CS3_TYPE 4 param set-default CA_SV_CS4_TYPE 9 param set-default CA_SV_CS5_TYPE 10 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1034_flightgear_rascal-electric b/ROMFS/px4fmu_common/init.d-posix/airframes/1034_flightgear_rascal-electric index e90fb1c65771..7cb6a3bf811e 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1034_flightgear_rascal-electric +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1034_flightgear_rascal-electric @@ -42,9 +42,9 @@ param set-default CA_SV_CS0_TRQ_R -0.5 param set-default CA_SV_CS0_TYPE 1 param set-default CA_SV_CS1_TRQ_R 0.5 param set-default CA_SV_CS1_TYPE 2 -param set-default CA_SV_CS2_TRQ_P 1.0 +param set-default CA_SV_CS2_TRQ_P 1 param set-default CA_SV_CS2_TYPE 3 -param set-default CA_SV_CS3_TRQ_Y 1.0 +param set-default CA_SV_CS3_TRQ_Y 1 param set-default CA_SV_CS3_TYPE 4 param set-default CA_SV_CS4_TYPE 9 param set-default CA_SV_CS5_TYPE 10 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1035_gazebo-classic_techpod b/ROMFS/px4fmu_common/init.d-posix/airframes/1035_gazebo-classic_techpod index 2c2d18380677..a8b29db64c6b 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1035_gazebo-classic_techpod +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1035_gazebo-classic_techpod @@ -40,9 +40,9 @@ param set-default CA_SV_CS0_TRQ_R -0.5 param set-default CA_SV_CS0_TYPE 1 param set-default CA_SV_CS1_TRQ_R 0.5 param set-default CA_SV_CS1_TYPE 2 -param set-default CA_SV_CS2_TRQ_P 1.0 +param set-default CA_SV_CS2_TRQ_P 1 param set-default CA_SV_CS2_TYPE 3 -param set-default CA_SV_CS3_TRQ_Y 1.0 +param set-default CA_SV_CS3_TRQ_Y 1 param set-default CA_SV_CS3_TYPE 4 param set-default CA_SV_CS4_TYPE 9 param set-default CA_SV_CS5_TYPE 10 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1036_jsbsim_malolo b/ROMFS/px4fmu_common/init.d-posix/airframes/1036_jsbsim_malolo index 89675aed623d..06a845be30a3 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1036_jsbsim_malolo +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1036_jsbsim_malolo @@ -42,9 +42,9 @@ param set-default CA_SV_CS0_TRQ_R -0.5 param set-default CA_SV_CS0_TYPE 1 param set-default CA_SV_CS1_TRQ_R 0.5 param set-default CA_SV_CS1_TYPE 2 -param set-default CA_SV_CS2_TRQ_P 1.0 +param set-default CA_SV_CS2_TRQ_P 1 param set-default CA_SV_CS2_TYPE 3 -param set-default CA_SV_CS3_TRQ_Y 1.0 +param set-default CA_SV_CS3_TRQ_Y 1 param set-default CA_SV_CS3_TYPE 4 param set-default CA_SV_CS4_TYPE 9 param set-default CA_SV_CS5_TYPE 10 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1037_gazebo-classic_believer b/ROMFS/px4fmu_common/init.d-posix/airframes/1037_gazebo-classic_believer index fc329d376eb0..83f63cec3842 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1037_gazebo-classic_believer +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1037_gazebo-classic_believer @@ -48,9 +48,9 @@ param set-default CA_SV_CS0_TRQ_R -0.5 param set-default CA_SV_CS0_TYPE 1 param set-default CA_SV_CS1_TRQ_R 0.5 param set-default CA_SV_CS1_TYPE 2 -param set-default CA_SV_CS2_TRQ_P 1.0 +param set-default CA_SV_CS2_TRQ_P 1 param set-default CA_SV_CS2_TYPE 3 -param set-default CA_SV_CS3_TRQ_Y 1.0 +param set-default CA_SV_CS3_TRQ_Y 1 param set-default CA_SV_CS3_TYPE 4 param set-default CA_SV_CS4_TYPE 9 param set-default CA_SV_CS5_TYPE 10 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1038_gazebo-classic_glider b/ROMFS/px4fmu_common/init.d-posix/airframes/1038_gazebo-classic_glider index 294c07b3efc9..d5b59f700792 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1038_gazebo-classic_glider +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1038_gazebo-classic_glider @@ -55,9 +55,9 @@ param set-default CA_SV_CS0_TRQ_R -0.5 param set-default CA_SV_CS0_TYPE 1 param set-default CA_SV_CS1_TRQ_R 0.5 param set-default CA_SV_CS1_TYPE 2 -param set-default CA_SV_CS2_TRQ_P 1.0 +param set-default CA_SV_CS2_TRQ_P 1 param set-default CA_SV_CS2_TYPE 3 -param set-default CA_SV_CS3_TRQ_Y 1.0 +param set-default CA_SV_CS3_TRQ_Y 1 param set-default CA_SV_CS3_TYPE 4 param set-default CA_SV_CS4_TYPE 9 param set-default CA_SV_CS5_TYPE 10 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1039_flightgear_rascal b/ROMFS/px4fmu_common/init.d-posix/airframes/1039_flightgear_rascal index 490d15b9c7f6..836615e635a2 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1039_flightgear_rascal +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1039_flightgear_rascal @@ -47,9 +47,9 @@ param set-default CA_SV_CS0_TRQ_R -0.5 param set-default CA_SV_CS0_TYPE 1 param set-default CA_SV_CS1_TRQ_R 0.5 param set-default CA_SV_CS1_TYPE 2 -param set-default CA_SV_CS2_TRQ_P 1.0 +param set-default CA_SV_CS2_TRQ_P 1 param set-default CA_SV_CS2_TYPE 3 -param set-default CA_SV_CS3_TRQ_Y 1.0 +param set-default CA_SV_CS3_TRQ_Y 1 param set-default CA_SV_CS3_TYPE 4 param set-default CA_SV_CS4_TYPE 9 param set-default CA_SV_CS5_TYPE 10 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 a998b8783108..1fafd1d4880f 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 @@ -45,9 +45,9 @@ param set-default CA_SV_CS0_TRQ_R -0.5 param set-default CA_SV_CS0_TYPE 1 param set-default CA_SV_CS1_TRQ_R 0.5 param set-default CA_SV_CS1_TYPE 2 -param set-default CA_SV_CS2_TRQ_P 1.0 +param set-default CA_SV_CS2_TRQ_P 1 param set-default CA_SV_CS2_TYPE 3 -param set-default CA_SV_CS3_TRQ_Y 1.0 +param set-default CA_SV_CS3_TRQ_Y 1 param set-default CA_SV_CS3_TYPE 4 param set-default CA_SV_CS4_TYPE 9 param set-default CA_SV_CS5_TYPE 10 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1040_gazebo-classic_standard_vtol b/ROMFS/px4fmu_common/init.d-posix/airframes/1040_gazebo-classic_standard_vtol index 9ad99c1d83ba..0e25b5633037 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1040_gazebo-classic_standard_vtol +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1040_gazebo-classic_standard_vtol @@ -27,8 +27,8 @@ param set-default CA_ROTOR2_KM -0.05 param set-default CA_ROTOR3_PX -0.1515 param set-default CA_ROTOR3_PY 0.1875 param set-default CA_ROTOR3_KM -0.05 -param set-default CA_ROTOR4_AX 1.0 -param set-default CA_ROTOR4_AZ 0.0 +param set-default CA_ROTOR4_AX 1 +param set-default CA_ROTOR4_AZ 0 param set-default CA_ROTOR4_PX 0.2 param set-default CA_SV_CS_COUNT 3 @@ -37,7 +37,7 @@ param set-default CA_SV_CS0_TRQ_R -0.5 param set-default CA_SV_CS1_TYPE 2 param set-default CA_SV_CS1_TRQ_R 0.5 param set-default CA_SV_CS2_TYPE 3 -param set-default CA_SV_CS2_TRQ_P 1.0 +param set-default CA_SV_CS2_TRQ_P 1 param set-default PWM_MAIN_FUNC1 101 param set-default PWM_MAIN_FUNC2 102 param set-default PWM_MAIN_FUNC3 103 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1041_gazebo-classic_tailsitter b/ROMFS/px4fmu_common/init.d-posix/airframes/1041_gazebo-classic_tailsitter index 1fe1fdde6e33..8a3d5d1a4965 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1041_gazebo-classic_tailsitter +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1041_gazebo-classic_tailsitter @@ -50,11 +50,11 @@ param set-default SENS_IMU_MODE 1 param set-default FW_P_TC 0.6 -param set-default FW_PR_FF 0.1 +param set-default FW_PR_FF 0.0 param set-default FW_PSP_OFF 2 param set-default FW_RR_FF 0.1 param set-default FW_RR_I 0.2 -param set-default FW_RR_P 0.3 +param set-default FW_RR_P 0.5 param set-default FW_THR_TRIM 0.35 param set-default FW_THR_MAX 0.8 param set-default FW_THR_MIN 0.05 @@ -66,8 +66,6 @@ param set-default FW_T_SINK_MIN 1.6 param set-default MC_AIRMODE 1 param set-default MC_ROLL_P 3 param set-default MC_PITCH_P 3 -param set-default MC_ROLLRATE_P 0.3 -param set-default MC_PITCHRATE_P 0.3 param set-default VT_ARSP_TRANS 10 param set-default VT_B_TRANS_DUR 5 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1042_gazebo-classic_tiltrotor b/ROMFS/px4fmu_common/init.d-posix/airframes/1042_gazebo-classic_tiltrotor index 9293cd3140b1..060399be7690 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1042_gazebo-classic_tiltrotor +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1042_gazebo-classic_tiltrotor @@ -31,7 +31,7 @@ param set-default CA_SV_CS0_TRQ_R -0.5 param set-default CA_SV_CS0_TYPE 1 param set-default CA_SV_CS1_TRQ_R 0.5 param set-default CA_SV_CS1_TYPE 2 -param set-default CA_SV_CS2_TRQ_P 1.0 +param set-default CA_SV_CS2_TRQ_P 1 param set-default CA_SV_CS2_TYPE 3 param set-default CA_SV_CS_COUNT 3 param set-default CA_SV_TL_COUNT 2 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1043_gazebo-classic_standard_vtol_drop b/ROMFS/px4fmu_common/init.d-posix/airframes/1043_gazebo-classic_standard_vtol_drop index f029918529f2..29f09dc47c9d 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1043_gazebo-classic_standard_vtol_drop +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1043_gazebo-classic_standard_vtol_drop @@ -27,8 +27,8 @@ param set-default CA_ROTOR2_KM -0.05 param set-default CA_ROTOR3_PX -0.1515 param set-default CA_ROTOR3_PY 0.1875 param set-default CA_ROTOR3_KM -0.05 -param set-default CA_ROTOR4_AX 1.0 -param set-default CA_ROTOR4_AZ 0.0 +param set-default CA_ROTOR4_AX 1 +param set-default CA_ROTOR4_AZ 0 param set-default CA_ROTOR4_PX 0.2 param set-default CA_SV_CS_COUNT 3 @@ -37,7 +37,7 @@ param set-default CA_SV_CS0_TRQ_R -0.5 param set-default CA_SV_CS1_TYPE 2 param set-default CA_SV_CS1_TRQ_R 0.5 param set-default CA_SV_CS2_TYPE 3 -param set-default CA_SV_CS2_TRQ_P 1.0 +param set-default CA_SV_CS2_TRQ_P 1 param set-default PWM_MAIN_FUNC1 101 param set-default PWM_MAIN_FUNC2 102 param set-default PWM_MAIN_FUNC3 103 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1044_gazebo-classic_plane_lidar b/ROMFS/px4fmu_common/init.d-posix/airframes/1044_gazebo-classic_plane_lidar index a92e0cf2c806..22d7e21119ee 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1044_gazebo-classic_plane_lidar +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1044_gazebo-classic_plane_lidar @@ -57,9 +57,9 @@ param set-default CA_SV_CS0_TRQ_R -0.5 param set-default CA_SV_CS0_TYPE 1 param set-default CA_SV_CS1_TRQ_R 0.5 param set-default CA_SV_CS1_TYPE 2 -param set-default CA_SV_CS2_TRQ_P 1.0 +param set-default CA_SV_CS2_TRQ_P 1 param set-default CA_SV_CS2_TYPE 3 -param set-default CA_SV_CS3_TRQ_Y 1.0 +param set-default CA_SV_CS3_TRQ_Y 1 param set-default CA_SV_CS3_TYPE 4 param set-default CA_SV_CS4_TYPE 9 param set-default CA_SV_CS5_TYPE 10 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1060_gazebo-classic_rover b/ROMFS/px4fmu_common/init.d-posix/airframes/1060_gazebo-classic_rover index d58f5aebaec1..ab8d12109205 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1060_gazebo-classic_rover +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1060_gazebo-classic_rover @@ -22,7 +22,7 @@ param set-default NAV_ACC_RAD 0.5 param set-default NAV_LOITER_RAD 2 param set-default GND_MAX_ANG 0.6 -param set-default GND_WHEEL_BASE 2.0 +param set-default GND_WHEEL_BASE 2 param set-default CA_AIRFRAME 5 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1062_flightgear_tf-r1 b/ROMFS/px4fmu_common/init.d-posix/airframes/1062_flightgear_tf-r1 index 40c5cb8d04f9..f3a6cd1aded5 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1062_flightgear_tf-r1 +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1062_flightgear_tf-r1 @@ -29,7 +29,7 @@ param set-default NAV_ACC_RAD 0.5 param set-default NAV_LOITER_RAD 2 param set-default GND_MAX_ANG 0.6 -param set-default GND_WHEEL_BASE 3.0 +param set-default GND_WHEEL_BASE 3 param set-default CA_AIRFRAME 5 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1070_gazebo-classic_boat b/ROMFS/px4fmu_common/init.d-posix/airframes/1070_gazebo-classic_boat index c173598078c8..01ada36f3510 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1070_gazebo-classic_boat +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1070_gazebo-classic_boat @@ -22,7 +22,7 @@ param set-default NAV_ACC_RAD 0.5 param set-default NAV_LOITER_RAD 2 param set-default GND_MAX_ANG 0.6 -param set-default GND_WHEEL_BASE 2.0 +param set-default GND_WHEEL_BASE 2 param set-default CA_AIRFRAME 9 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/17001_flightgear_tf-g1 b/ROMFS/px4fmu_common/init.d-posix/airframes/17001_flightgear_tf-g1 index f733f9cf25f2..9c9b2c7f6312 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/17001_flightgear_tf-g1 +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/17001_flightgear_tf-g1 @@ -14,7 +14,7 @@ param set-default FW_AIRSPD_STALL 8 -param set-default FW_P_RMAX_NEG 20.0 +param set-default FW_P_RMAX_NEG 20 param set-default FW_W_RMAX 10 param set-default FW_W_EN 1 @@ -37,7 +37,7 @@ param set-default FW_P_LIM_MAX 25 param set-default FW_P_LIM_MIN -5 param set-default FW_R_LIM 30 -param set-default FW_MAN_R_MAX 30.0 +param set-default FW_MAN_R_MAX 30 param set-default FW_THR_TRIM 0.8 param set-default FW_THR_IDLE 0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/17002_flightgear_tf-g2 b/ROMFS/px4fmu_common/init.d-posix/airframes/17002_flightgear_tf-g2 index 5ed2fbf9667d..74b6d2a31679 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/17002_flightgear_tf-g2 +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/17002_flightgear_tf-g2 @@ -14,7 +14,7 @@ param set-default FW_AIRSPD_STALL 5 -param set-default FW_P_RMAX_NEG 20.0 +param set-default FW_P_RMAX_NEG 20 param set-default FW_W_RMAX 10 param set-default FW_W_EN 1 @@ -29,9 +29,9 @@ param set-default NAV_LOITER_RAD 50 # Parameters related to autogyro takeoff PR #param set-default AG_TKOFF 1 #param set-default AG_PROT_TYPE 1 -#param set-default AG_PROT_MIN_RPM 50.0 -#param set-default AG_PROT_TRG_RPM 900.0 -#param set-defoult AG_ROTOR_RPM 900.0 +#param set-default AG_PROT_MIN_RPM 50 +#param set-default AG_PROT_TRG_RPM 900 +#param set-defoult AG_ROTOR_RPM 900 param set-default FW_ARSP_SCALE_EN 0 @@ -42,7 +42,7 @@ param set-default FW_P_LIM_MAX 25 param set-default FW_P_LIM_MIN -5 param set-default FW_R_LIM 30 -param set-default FW_MAN_R_MAX 30.0 +param set-default FW_MAN_R_MAX 30 param set-default FW_THR_CRUISE 0.8 param set-default FW_THR_IDLE 0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/2507_gazebo-classic_cloudship b/ROMFS/px4fmu_common/init.d-posix/airframes/2507_gazebo-classic_cloudship index 01f7c2ac2b48..153247203da6 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/2507_gazebo-classic_cloudship +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/2507_gazebo-classic_cloudship @@ -14,21 +14,21 @@ param set-default CA_AIRFRAME 9 param set-default CA_ROTOR_COUNT 3 -param set-default CA_ROTOR0_AX 1.0000 -param set-default CA_ROTOR0_AZ 0.0000 -param set-default CA_ROTOR0_KM 0.0000 -param set-default CA_ROTOR0_PY 2.0000 -param set-default CA_ROTOR1_AX 1.0000 -param set-default CA_ROTOR1_AZ 0.0000 -param set-default CA_ROTOR1_KM 0.0000 -param set-default CA_ROTOR1_PY -2.0000 -param set-default CA_ROTOR2_AY -1.0000 -param set-default CA_ROTOR2_AZ 0.0000 -param set-default CA_ROTOR2_KM 0.0000 -param set-default CA_ROTOR2_PX -10.0000 +param set-default CA_ROTOR0_AX 1 +param set-default CA_ROTOR0_AZ 0 +param set-default CA_ROTOR0_KM 0 +param set-default CA_ROTOR0_PY 2 +param set-default CA_ROTOR1_AX 1 +param set-default CA_ROTOR1_AZ 0 +param set-default CA_ROTOR1_KM 0 +param set-default CA_ROTOR1_PY -2 +param set-default CA_ROTOR2_AY -1 +param set-default CA_ROTOR2_AZ 0 +param set-default CA_ROTOR2_KM 0 +param set-default CA_ROTOR2_PX -10 param set-default CA_SV_CS_COUNT 1 -param set-default CA_SV_CS0_TRQ_P 1.0000 +param set-default CA_SV_CS0_TRQ_P 1 param set-default CA_R_REV 7 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/3011_jsbsim_hexarotor_x b/ROMFS/px4fmu_common/init.d-posix/airframes/3011_jsbsim_hexarotor_x index b17de91d1b57..a3fd956dfd59 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/3011_jsbsim_hexarotor_x +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/3011_jsbsim_hexarotor_x @@ -13,9 +13,9 @@ param set-default MAV_TYPE 13 param set-default MC_PITCHRATE_P 0.1 param set-default MC_PITCHRATE_I 0.05 -param set-default MC_PITCH_P 6.0 +param set-default MC_PITCH_P 6 param set-default MC_ROLLRATE_I 0.1 -param set-default MC_ROLL_P 6.0 +param set-default MC_ROLL_P 6 param set-default MPC_XY_VEL_I_ACC 4 param set-default MPC_XY_VEL_P_ACC 3 @@ -27,11 +27,11 @@ param set-default MNT_MODE_IN 4 param set-default MNT_DO_STAB 2 -param set-default CA_ROTOR0_PX 0.0 -param set-default CA_ROTOR0_PY 1.0 +param set-default CA_ROTOR0_PX 0 +param set-default CA_ROTOR0_PY 1 param set-default CA_ROTOR0_KM -0.05 -param set-default CA_ROTOR1_PX 0.0 -param set-default CA_ROTOR1_PY -1.0 +param set-default CA_ROTOR1_PX 0 +param set-default CA_ROTOR1_PY -1 param set-default CA_ROTOR1_KM 0.05 param set-default CA_ROTOR2_PX 0.866025 param set-default CA_ROTOR2_PY -0.5 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna b/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna index e87e013e5d77..d7ea1f3b37a4 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna @@ -70,9 +70,9 @@ param set-default CA_SV_CS0_TRQ_R -0.5 param set-default CA_SV_CS0_TYPE 1 param set-default CA_SV_CS1_TRQ_R 0.5 param set-default CA_SV_CS1_TYPE 2 -param set-default CA_SV_CS2_TRQ_P 1.0 +param set-default CA_SV_CS2_TRQ_P 1 param set-default CA_SV_CS2_TYPE 3 -param set-default CA_SV_CS3_TRQ_Y 1.0 +param set-default CA_SV_CS3_TRQ_Y 1 param set-default CA_SV_CS3_TYPE 4 param set-default CA_SV_CS4_TYPE 9 param set-default CA_SV_CS5_TYPE 10 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol b/ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol index 57fd1d68669d..3390ee525cc4 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol @@ -40,8 +40,8 @@ param set-default CA_ROTOR2_KM -0.05 param set-default CA_ROTOR3_PX -0.1515 param set-default CA_ROTOR3_PY 0.1875 param set-default CA_ROTOR3_KM -0.05 -param set-default CA_ROTOR4_AX 1.0 -param set-default CA_ROTOR4_AZ 0.0 +param set-default CA_ROTOR4_AX 1 +param set-default CA_ROTOR4_AZ 0 param set-default CA_ROTOR4_PX 0.2 param set-default SIM_GZ_EC_FUNC1 101 @@ -73,7 +73,7 @@ param set-default CA_SV_CS0_TRQ_R -0.5 param set-default CA_SV_CS1_TYPE 2 param set-default CA_SV_CS1_TRQ_R 0.5 param set-default CA_SV_CS2_TYPE 3 -param set-default CA_SV_CS2_TRQ_P 1.0 +param set-default CA_SV_CS2_TRQ_P 1 param set-default NPFG_PERIOD 12 param set-default FW_PR_FF 0.2 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision b/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision index 2b67f387f223..92560eb160df 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision @@ -105,31 +105,19 @@ param set-default CBRK_IO_SAFETY 22027 param set-default THR_MDL_FAC 0.3 +# Square quadrotor X PX4 numbering param set-default CA_ROTOR_COUNT 4 -param set-default CA_ROTOR0_PX 0.15 -param set-default CA_ROTOR0_PY 0.15 -param set-default CA_ROTOR0_KM 0.05 -param set-default CA_ROTOR1_PX -0.15 -param set-default CA_ROTOR1_PY -0.15 -param set-default CA_ROTOR1_KM 0.05 -param set-default CA_ROTOR2_PX 0.15 -param set-default CA_ROTOR2_PY -0.15 +param set-default CA_ROTOR0_PX 1 +param set-default CA_ROTOR0_PY 1 +param set-default CA_ROTOR1_PX -1 +param set-default CA_ROTOR1_PY -1 +param set-default CA_ROTOR2_PX 1 +param set-default CA_ROTOR2_PY -1 param set-default CA_ROTOR2_KM -0.05 -param set-default CA_ROTOR3_PX -0.15 -param set-default CA_ROTOR3_PY 0.15 +param set-default CA_ROTOR3_PX -1 +param set-default CA_ROTOR3_PY 1 param set-default CA_ROTOR3_KM -0.05 -# Outputs -# 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_MAIN_FUNC1 101 -# param set-default PWM_MAIN_FUNC2 102 -# param set-default PWM_MAIN_FUNC3 103 -# param set-default PWM_MAIN_FUNC4 104 - param set-default SIM_GZ_EC_FUNC1 101 param set-default SIM_GZ_EC_FUNC2 102 param set-default SIM_GZ_EC_FUNC3 103 @@ -144,5 +132,3 @@ 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 - - diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4008_gz_advanced_plane b/ROMFS/px4fmu_common/init.d-posix/airframes/4008_gz_advanced_plane index 37a407383fce..2a49b085416f 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4008_gz_advanced_plane +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4008_gz_advanced_plane @@ -63,9 +63,9 @@ param set-default CA_SV_CS0_TRQ_R -0.5 param set-default CA_SV_CS0_TYPE 1 param set-default CA_SV_CS1_TRQ_R 0.5 param set-default CA_SV_CS1_TYPE 2 -param set-default CA_SV_CS2_TRQ_P 1.0 +param set-default CA_SV_CS2_TRQ_P 1 param set-default CA_SV_CS2_TYPE 3 -param set-default CA_SV_CS3_TRQ_Y 1.0 +param set-default CA_SV_CS3_TRQ_Y 1 param set-default CA_SV_CS3_TYPE 4 param set-default CA_SV_CS4_TYPE 9 param set-default CA_SV_CS5_TYPE 10 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower b/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower new file mode 100644 index 000000000000..1dcb917a09ad --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower @@ -0,0 +1,80 @@ +#!/bin/sh +# @name Gazebo lawnmower +# @type Rover +# @class Rover + +. ${R}etc/init.d/rc.rover_differential_defaults + +PX4_SIMULATOR=${PX4_SIMULATOR:=gz} +PX4_GZ_WORLD=${PX4_GZ_WORLD:=default} +PX4_SIM_MODEL=${PX4_SIM_MODEL:=lawnmower} + +param set-default SIM_GZ_EN 1 # Gazebo bridge + +# Simulated sensors +param set-default SENS_EN_GPSSIM 1 +param set-default SENS_EN_BAROSIM 0 +param set-default SENS_EN_MAGSIM 1 +param set-default SENS_EN_ARSPDSIM 1 +# We can arm and drive in manual mode when it slides and GPS check fails: +param set-default COM_ARM_WO_GPS 1 + +# Set Differential Drive Kinematics Library parameters: +param set RDD_WHEEL_BASE 0.9 +param set RDD_WHEEL_RADIUS 0.22 +param set RDD_WHEEL_SPEED 10.0 # Maximum wheel speed rad/s, approx 8 km/h + +# Actuator mapping - set SITL motors/servos output parameters: + +# "Motors" - motor channels 0 (Right) and 1 (Left) - via Wheels GZ bridge: +param set-default SIM_GZ_WH_FUNC1 101 # right wheel +#param set-default SIM_GZ_WH_MIN1 0 +#param set-default SIM_GZ_WH_MAX1 200 +#param set-default SIM_GZ_WH_DIS1 100 +#param set-default SIM_GZ_WH_FAIL1 100 + +param set-default SIM_GZ_WH_FUNC2 102 # left wheel +#param set-default SIM_GZ_WH_MIN2 0 +#param set-default SIM_GZ_WH_MAX2 200 +#aram set-default SIM_GZ_WH_DIS2 100 +#param set-default SIM_GZ_WH_FAIL2 100 + +param set-default SIM_GZ_WH_REV 0 # no need to reverse any wheels + +# Note: The servo configurations ( SIM_GZ_SV_FUNC*) outlined below are intended for educational purposes in this simulation. +# They do not have physical effects in the simulated environment, except for actuating the joints. Their definitions are meant to demonstrate +# how actuators could be mapped and configured in a real-world application, providing a foundation for understanding and implementing actuator +# controls in practical scenarios. + +# Cutter deck blades clutch, PCA9685 servo channel 3, "RC FLAPS" (406) - leftmost switch, or "Servo 3" (203): +param set-default SIM_GZ_SV_FUNC3 203 +param set-default SIM_GZ_SV_MIN3 0 +param set-default SIM_GZ_SV_MAX3 1000 +param set-default SIM_GZ_SV_DIS3 500 +param set-default SIM_GZ_SV_FAIL3 500 + +# Gas engine throttle, PCA9685 servo channel 4, "RC AUX1" (407) - left knob, or "Servo 4" (204): +# - on minimum when disarmed or failed: +param set-default SIM_GZ_SV_FUNC4 204 +param set-default SIM_GZ_SV_MIN4 0 +param set-default SIM_GZ_SV_MAX4 1000 +param set-default SIM_GZ_SV_DIS4 500 +param set-default SIM_GZ_SV_FAIL4 500 + +# Controlling PCA9685 servos 5,6,7,8 directly via "Servo 5..8" setting, by publishing actuator_servos.control[]: + +# Strobes, PCA9685 servo channel 5, "Servo 5" (205) - flashing indicates Mission mode: +#param set-default SIM_GZ_SV_FUNC5 205 +#param set-default SIM_GZ_SV_MIN5 1000 +#param set-default SIM_GZ_SV_MAX5 2000 +#param set-default SIM_GZ_SV_DIS5 1000 +#param set-default SIM_GZ_SV_FAIL5 1000 + +# Horn, PCA9685 servo channel 6, "Servo 6" (206) - for alarms like GPS failure: +#param set-default SIM_GZ_SV_FUNC6 206 + +# Spare PCA9685 servo channel 7 on "RC AUX2" (408) - right knob, or "Servo 7" (207): +#param set-default SIM_GZ_SV_FUNC7 207 + +# Spare PCA9685 servo channel 8 - "Servo 8" (208): +#param set-default SIM_GZ_SV_FUNC8 208 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/6011_gazebo-classic_typhoon_h480 b/ROMFS/px4fmu_common/init.d-posix/airframes/6011_gazebo-classic_typhoon_h480 index 05ae1af81a5a..0174411c73a6 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/6011_gazebo-classic_typhoon_h480 +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/6011_gazebo-classic_typhoon_h480 @@ -9,14 +9,14 @@ param set-default MAV_TYPE 13 -param set-default MC_PITCHRATE_P 0.0800 -param set-default MC_PITCHRATE_I 0.0400 -param set-default MC_PITCHRATE_D 0.0010 -param set-default MC_PITCH_P 9.0 -param set-default MC_ROLLRATE_P 0.0800 -param set-default MC_ROLLRATE_I 0.0400 -param set-default MC_ROLLRATE_D 0.0010 -param set-default MC_ROLL_P 9.0 +param set-default MC_PITCHRATE_P 0.08 +param set-default MC_PITCHRATE_I 0.04 +param set-default MC_PITCHRATE_D 0.001 +param set-default MC_PITCH_P 9 +param set-default MC_ROLLRATE_P 0.08 +param set-default MC_ROLLRATE_I 0.04 +param set-default MC_ROLLRATE_D 0.001 +param set-default MC_ROLL_P 9 param set-default MPC_XY_VEL_I_ACC 4 param set-default MPC_XY_VEL_P_ACC 3 @@ -31,11 +31,11 @@ param set-default MAV_PROTO_VER 2 param set-default CA_AIRFRAME 0 param set-default CA_ROTOR_COUNT 6 -param set-default CA_ROTOR0_PX 0.0 -param set-default CA_ROTOR0_PY 1.0 +param set-default CA_ROTOR0_PX 0 +param set-default CA_ROTOR0_PY 1 param set-default CA_ROTOR0_KM -0.05 -param set-default CA_ROTOR1_PX 0.0 -param set-default CA_ROTOR1_PY -1.0 +param set-default CA_ROTOR1_PX 0 +param set-default CA_ROTOR1_PY -1 param set-default CA_ROTOR1_KM 0.05 param set-default CA_ROTOR2_PX 0.866025 param set-default CA_ROTOR2_PY -0.5 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index ed460d992993..9277ad303947 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -82,6 +82,7 @@ px4_add_romfs_files( 4008_gz_advanced_plane 4009_gz_r1_rover 4010_gz_x500_mono_cam + 4011_gz_lawnmower 6011_gazebo-classic_typhoon_h480 6011_gazebo-classic_typhoon_h480.post diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator index d8ea0f3b95a8..8121f3914c2a 100644 --- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator +++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator @@ -41,19 +41,6 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then echo "INFO [init] Gazebo simulator" - # 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_ALT} - fi - # Only start up Gazebo if PX4_GZ_STANDALONE is not set. if [ -z "${PX4_GZ_STANDALONE}" ]; then @@ -64,6 +51,13 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then # "gz sim" from Garden on gz_command="gz" gz_sub_command="sim" + + # Specify render engine if `GZ_SIM_RENDER_ENGINE` is set + # (for example, if you want to use Ogre 1.x instead of Ogre 2.x): + if [ -n "${PX4_GZ_SIM_RENDER_ENGINE}" ]; then + echo "INFO [init] Setting Gazebo render engine to '${PX4_GZ_SIM_RENDER_ENGINE}'!" + gz_sub_command="${gz_sub_command} --render-engine ${PX4_GZ_SIM_RENDER_ENGINE}" + fi else echo "ERROR [init] Gazebo gz please install gz-garden" exit 1 @@ -173,6 +167,12 @@ elif [ "$PX4_SIM_MODEL" = "jmavsim_iris" ] || [ "$(param show -q SYS_AUTOSTART)" else # otherwise start simulator (mavlink) module + + # EKF2 specifics + param set-default EKF2_GPS_DELAY 10 + param set-default EKF2_MULTI_IMU 3 + param set-default SENS_IMU_MODE 0 + simulator_tcp_port=$((4560+px4_instance)) # Check if PX4_SIM_HOSTNAME environment variable is empty diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index dbd837d40b04..0084a6701e6d 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -164,10 +164,6 @@ param set-default COM_RC_IN_MODE 1 # Speedup SITL startup param set-default EKF2_REQ_GPS_H 0.5 -# Multi-EKF -param set-default EKF2_MULTI_IMU 3 -param set-default SENS_IMU_MODE 0 - param set-default IMU_GYRO_FFT_EN 1 param set-default MAV_PROTO_VER 2 # Ensures QGC does not drop the first few packets after a SITL restart due to MAVLINK 1 packets @@ -248,7 +244,12 @@ then fi load_mon start -battery_simulator start + +if param compare SIM_BAT_ENABLE 1 +then + battery_simulator start +fi + tone_alarm start rc_update start manual_control start diff --git a/ROMFS/px4fmu_common/init.d/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/CMakeLists.txt index 4b45187ff5b7..34b255e2555f 100644 --- a/ROMFS/px4fmu_common/init.d/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/CMakeLists.txt @@ -34,28 +34,79 @@ add_subdirectory(airframes) px4_add_romfs_files( - rc.airship_apps - rc.airship_defaults + rc.autostart_ext - rc.balloon_apps - rc.balloon_defaults - rc.boat_defaults - rc.fw_apps - rc.fw_defaults - rc.heli_defaults - rc.logging - rc.mc_apps - rc.mc_defaults + rcS rc.sensors - rc.thermal_cal - rc.rover_apps - rc.rover_defaults - rc.rover_differential_apps - rc.rover_differential_defaults - rc.uuv_apps - rc.uuv_defaults rc.vehicle_setup - rc.vtol_apps - rc.vtol_defaults + + # TODO + rc.balloon_apps + rc.balloon_defaults ) + +if(CONFIG_MODULES_AIRSHIP_ATT_CONTROL) + px4_add_romfs_files( + rc.airship_apps + rc.airship_defaults + ) +endif() + +if(CONFIG_MODULES_FW_RATE_CONTROL) + px4_add_romfs_files( + rc.fw_apps + rc.fw_defaults + ) +endif() + +if(CONFIG_MODULES_MC_RATE_CONTROL) + px4_add_romfs_files( + rc.heli_defaults + rc.mc_apps + rc.mc_defaults + ) +endif() + +if(CONFIG_MODULES_ROVER_POS_CONTROL) + px4_add_romfs_files( + rc.rover_apps + rc.rover_defaults + + rc.boat_defaults # hack + ) +endif() + +if(CONFIG_MODULES_DIFFERENTIAL_DRIVE) + px4_add_romfs_files( + rc.rover_differential_apps + rc.rover_differential_defaults + ) +endif() + +if(CONFIG_MODULES_UUV_ATT_CONTROL) + px4_add_romfs_files( + rc.uuv_apps + rc.uuv_defaults + ) +endif() + +if(CONFIG_MODULES_VTOL_ATT_CONTROL) + px4_add_romfs_files( + rc.vtol_apps + rc.vtol_defaults + ) +endif() + + +if(CONFIG_MODULES_LOGGER) + px4_add_romfs_files( + rc.logging + ) +endif() + +if(CONFIG_MODULES_TEMPERATURE_COMPENSATION) + px4_add_romfs_files( + rc.thermal_cal + ) +endif() diff --git a/ROMFS/px4fmu_common/init.d/airframes/1001_rc_quad_x.hil b/ROMFS/px4fmu_common/init.d/airframes/1001_rc_quad_x.hil index 9cbf0286603b..1b04f904c9be 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1001_rc_quad_x.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1001_rc_quad_x.hil @@ -11,30 +11,30 @@ . ${R}etc/init.d/rc.mc_defaults - param set SYS_HITL 1 param set UAVCAN_ENABLE 0 +# disable some checks to allow to fly +# - without real battery +param set-default CBRK_SUPPLY_CHK 894281 +# - without safety switch +param set-default CBRK_IO_SAFETY 22027 + +# Square quadrotor X PX4 numbering param set-default CA_ROTOR_COUNT 4 -param set-default CA_ROTOR0_PX 0.15 -param set-default CA_ROTOR0_PY 0.15 -param set-default CA_ROTOR1_PX -0.15 -param set-default CA_ROTOR1_PY -0.15 -param set-default CA_ROTOR2_PX 0.15 -param set-default CA_ROTOR2_PY -0.15 +param set-default CA_ROTOR0_PX 1 +param set-default CA_ROTOR0_PY 1 +param set-default CA_ROTOR1_PX -1 +param set-default CA_ROTOR1_PY -1 +param set-default CA_ROTOR2_PX 1 +param set-default CA_ROTOR2_PY -1 param set-default CA_ROTOR2_KM -0.05 -param set-default CA_ROTOR3_PX -0.15 -param set-default CA_ROTOR3_PY 0.15 +param set-default CA_ROTOR3_PX -1 +param set-default CA_ROTOR3_PY 1 param set-default CA_ROTOR3_KM -0.05 param set-default HIL_ACT_FUNC1 101 param set-default HIL_ACT_FUNC2 102 param set-default HIL_ACT_FUNC3 103 param set-default HIL_ACT_FUNC4 104 - -# disable some checks to allow to fly -# - without real battery -param set-default CBRK_SUPPLY_CHK 894281 -# - without safety switch -param set-default CBRK_IO_SAFETY 22027 diff --git a/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil b/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil index 60a01fb403e6..461120e57203 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil @@ -14,7 +14,6 @@ . ${R}etc/init.d/rc.vtol_defaults - param set-default BAT1_N_CELLS 3 param set-default COM_RC_IN_MODE 1 @@ -55,26 +54,29 @@ param set-default VT_TYPE 2 param set-default CA_AIRFRAME 2 param set-default CA_ROTOR_COUNT 5 -param set-default CA_ROTOR0_PX 0.15 -param set-default CA_ROTOR0_PY 0.15 -param set-default CA_ROTOR1_PX -0.15 -param set-default CA_ROTOR1_PY -0.15 -param set-default CA_ROTOR2_PX 0.15 -param set-default CA_ROTOR2_PY -0.15 +param set-default CA_ROTOR4_AX 1 +param set-default CA_ROTOR4_AZ 0 +param set-default CA_ROTOR4_PX 0.2 + +# Square quadrotor X PX4 numbering +param set-default CA_ROTOR_COUNT 4 +param set-default CA_ROTOR0_PX 1 +param set-default CA_ROTOR0_PY 1 +param set-default CA_ROTOR1_PX -1 +param set-default CA_ROTOR1_PY -1 +param set-default CA_ROTOR2_PX 1 +param set-default CA_ROTOR2_PY -1 param set-default CA_ROTOR2_KM -0.05 -param set-default CA_ROTOR3_PX -0.15 -param set-default CA_ROTOR3_PY 0.15 +param set-default CA_ROTOR3_PX -1 +param set-default CA_ROTOR3_PY 1 param set-default CA_ROTOR3_KM -0.05 -param set-default CA_ROTOR4_AX 1.0 -param set-default CA_ROTOR4_AZ 0.0 -param set-default CA_ROTOR4_PX 0.2 param set-default CA_SV_CS_COUNT 3 param set-default CA_SV_CS0_TRQ_R -0.5 param set-default CA_SV_CS0_TYPE 1 param set-default CA_SV_CS1_TRQ_R 0.5 param set-default CA_SV_CS1_TYPE 2 -param set-default CA_SV_CS2_TRQ_P 1.0 +param set-default CA_SV_CS2_TRQ_P 1 param set-default CA_SV_CS2_TYPE 3 param set-default HIL_ACT_FUNC1 101 @@ -98,5 +100,3 @@ param set-default CBRK_SUPPLY_CHK 894281 param set-default CBRK_IO_SAFETY 22027 param set-default MAV_TYPE 22 - - diff --git a/ROMFS/px4fmu_common/init.d/airframes/1100_rc_quad_x_sih.hil b/ROMFS/px4fmu_common/init.d/airframes/1100_rc_quad_x_sih.hil index b1b42bd3e35a..2aab1ebe47f6 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1100_rc_quad_x_sih.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1100_rc_quad_x_sih.hil @@ -12,26 +12,8 @@ . ${R}etc/init.d/rc.mc_defaults - param set UAVCAN_ENABLE 0 -param set-default CA_ROTOR_COUNT 4 -param set-default CA_ROTOR0_PX 0.15 -param set-default CA_ROTOR0_PY 0.15 -param set-default CA_ROTOR1_PX -0.15 -param set-default CA_ROTOR1_PY -0.15 -param set-default CA_ROTOR2_PX 0.15 -param set-default CA_ROTOR2_PY -0.15 -param set-default CA_ROTOR2_KM -0.05 -param set-default CA_ROTOR3_PX -0.15 -param set-default CA_ROTOR3_PY 0.15 -param set-default CA_ROTOR3_KM -0.05 - -param set-default HIL_ACT_FUNC1 101 -param set-default HIL_ACT_FUNC2 102 -param set-default HIL_ACT_FUNC3 103 -param set-default HIL_ACT_FUNC4 104 - # set SYS_HITL to 2 to start the SIH and avoid sensors startup param set SYS_HITL 2 @@ -42,3 +24,21 @@ param set-default CBRK_SUPPLY_CHK 894281 param set-default CBRK_IO_SAFETY 22027 param set SIH_VEHICLE_TYPE 0 + +# Square quadrotor X PX4 numbering +param set-default CA_ROTOR_COUNT 4 +param set-default CA_ROTOR0_PX 1 +param set-default CA_ROTOR0_PY 1 +param set-default CA_ROTOR1_PX -1 +param set-default CA_ROTOR1_PY -1 +param set-default CA_ROTOR2_PX 1 +param set-default CA_ROTOR2_PY -1 +param set-default CA_ROTOR2_KM -0.05 +param set-default CA_ROTOR3_PX -1 +param set-default CA_ROTOR3_PY 1 +param set-default CA_ROTOR3_KM -0.05 + +param set-default HIL_ACT_FUNC1 101 +param set-default HIL_ACT_FUNC2 102 +param set-default HIL_ACT_FUNC3 103 +param set-default HIL_ACT_FUNC4 104 diff --git a/ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil b/ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil index 0ec506b68f95..07c7d925c238 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil @@ -21,9 +21,9 @@ param set-default CA_ROTOR0_PX 0.3 param set-default CA_SV_CS_COUNT 4 param set-default CA_SV_CS0_TRQ_R 0.5 param set-default CA_SV_CS0_TYPE 2 -param set-default CA_SV_CS1_TRQ_P 1.0 +param set-default CA_SV_CS1_TRQ_P 1 param set-default CA_SV_CS1_TYPE 3 -param set-default CA_SV_CS2_TRQ_Y 1.0 +param set-default CA_SV_CS2_TRQ_Y 1 param set-default CA_SV_CS2_TYPE 4 param set-default CA_SV_CS3_TYPE 10 @@ -46,7 +46,7 @@ param set-default CBRK_IO_SAFETY 22027 param set-default BAT_N_CELLS 3 -param set SIH_T_MAX 6.0 +param set SIH_T_MAX 6 param set SIH_MASS 0.3 param set SIH_IXX 0.00402 param set SIH_IYY 0.0144 diff --git a/ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil b/ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil index d47d11545be5..6ac7bea691c4 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil @@ -71,7 +71,7 @@ param set SIH_MASS 0.2 param set SIH_IXX 0.00354 param set SIH_IYY 0.000625 param set SIH_IZZ 0.00300 -param set SIH_IXZ 0.0 +param set SIH_IXZ 0 param set SIH_KDV 0.2 param set SIH_L_ROLL 0.145 diff --git a/ROMFS/px4fmu_common/init.d/airframes/13000_generic_vtol_standard b/ROMFS/px4fmu_common/init.d/airframes/13000_generic_vtol_standard index 38dc721bc785..d360b3835e57 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13000_generic_vtol_standard +++ b/ROMFS/px4fmu_common/init.d/airframes/13000_generic_vtol_standard @@ -14,6 +14,21 @@ param set-default CA_AIRFRAME 2 param set-default CA_ROTOR_COUNT 5 +param set-default CA_ROTOR4_AX 1 +param set-default CA_ROTOR4_AZ 0 +param set-default CA_SV_CS_COUNT 4 +param set-default CA_SV_CS0_TYPE 1 +param set-default CA_SV_CS0_TRQ_R -0.5 +param set-default CA_SV_CS1_TYPE 2 +param set-default CA_SV_CS1_TRQ_R 0.5 +param set-default CA_SV_CS2_TYPE 3 +param set-default CA_SV_CS2_TRQ_P 1 +param set-default CA_SV_CS3_TRQ_Y 1 +param set-default CA_SV_CS3_TYPE 4 +param set-default VT_TYPE 2 +param set-default MAV_TYPE 22 + +# Square quadrotor X PX4 numbering param set-default CA_ROTOR0_PX 1 param set-default CA_ROTOR0_PY 1 param set-default CA_ROTOR1_PX -1 @@ -24,16 +39,3 @@ param set-default CA_ROTOR2_KM -0.05 param set-default CA_ROTOR3_PX -1 param set-default CA_ROTOR3_PY 1 param set-default CA_ROTOR3_KM -0.05 -param set-default CA_ROTOR4_AX 1.0 -param set-default CA_ROTOR4_AZ 0.0 -param set-default CA_SV_CS_COUNT 4 -param set-default CA_SV_CS0_TYPE 1 -param set-default CA_SV_CS0_TRQ_R -0.5 -param set-default CA_SV_CS1_TYPE 2 -param set-default CA_SV_CS1_TRQ_R 0.5 -param set-default CA_SV_CS2_TYPE 3 -param set-default CA_SV_CS2_TRQ_P 1.0 -param set-default CA_SV_CS3_TRQ_Y 1.0 -param set-default CA_SV_CS3_TYPE 4 -param set-default VT_TYPE 2 -param set-default MAV_TYPE 22 diff --git a/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad b/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad index a6d97aedc1e4..0b2ea2596637 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad +++ b/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad @@ -120,8 +120,8 @@ param set-default CA_ROTOR2_KM -0.05 param set-default CA_ROTOR3_PX -0.1515 param set-default CA_ROTOR3_PY 0.1875 param set-default CA_ROTOR3_KM -0.05 -param set-default CA_ROTOR4_AX 1.0 -param set-default CA_ROTOR4_AZ 0.0 +param set-default CA_ROTOR4_AX 1 +param set-default CA_ROTOR4_AZ 0 param set-default CA_ROTOR4_PX 0.2 param set-default CA_SV_CS_COUNT 3 @@ -130,4 +130,4 @@ param set-default CA_SV_CS0_TRQ_R -0.5 param set-default CA_SV_CS1_TYPE 2 param set-default CA_SV_CS1_TRQ_R 0.5 param set-default CA_SV_CS2_TYPE 3 -param set-default CA_SV_CS2_TRQ_P 1.0 +param set-default CA_SV_CS2_TRQ_P 1 diff --git a/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark b/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark index ac5931075862..cc8db21732a1 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark +++ b/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark @@ -80,6 +80,10 @@ param set-default VT_TYPE 2 param set-default CA_AIRFRAME 2 param set-default CA_ROTOR_COUNT 5 +param set-default CA_ROTOR4_AX 1 +param set-default CA_ROTOR4_AZ 0 + +# Square quadrotor X PX4 numbering param set-default CA_ROTOR0_PX 1 param set-default CA_ROTOR0_PY 1 param set-default CA_ROTOR1_PX -1 @@ -90,18 +94,16 @@ param set-default CA_ROTOR2_KM -0.05 param set-default CA_ROTOR3_PX -1 param set-default CA_ROTOR3_PY 1 param set-default CA_ROTOR3_KM -0.05 -param set-default CA_ROTOR4_AX 1.0 -param set-default CA_ROTOR4_AZ 0.0 param set-default CA_SV_CS_COUNT 3 param set-default CA_SV_CS0_TYPE 15 -param set-default CA_SV_CS0_TRQ_R 1.0 -param set-default CA_SV_CS1_TRQ_P 0.5000 -param set-default CA_SV_CS1_TRQ_R 0.0000 -param set-default CA_SV_CS1_TRQ_Y -0.5000 +param set-default CA_SV_CS0_TRQ_R 1 +param set-default CA_SV_CS1_TRQ_P 0.5 +param set-default CA_SV_CS1_TRQ_R 0 +param set-default CA_SV_CS1_TRQ_Y -0.5 param set-default CA_SV_CS1_TYPE 13 -param set-default CA_SV_CS2_TRQ_P 0.5000 -param set-default CA_SV_CS2_TRQ_Y 0.5000 +param set-default CA_SV_CS2_TRQ_P 0.5 +param set-default CA_SV_CS2_TRQ_Y 0.5 param set-default CA_SV_CS2_TYPE 14 param set-default PWM_MAIN_FUNC1 201 diff --git a/ROMFS/px4fmu_common/init.d/airframes/13030_generic_vtol_quad_tiltrotor b/ROMFS/px4fmu_common/init.d/airframes/13030_generic_vtol_quad_tiltrotor index 7c0c1af8a51f..783b6463919c 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13030_generic_vtol_quad_tiltrotor +++ b/ROMFS/px4fmu_common/init.d/airframes/13030_generic_vtol_quad_tiltrotor @@ -19,30 +19,30 @@ param set-default MAV_TYPE 21 param set-default VT_TYPE 1 param set-default CA_AIRFRAME 3 -param set-default CA_ROTOR_COUNT 4 -param set-default CA_ROTOR0_PX 0.15 -param set-default CA_ROTOR0_PY 0.15 param set-default CA_ROTOR0_TILT 1 -param set-default CA_ROTOR1_PX -0.15 -param set-default CA_ROTOR1_PY -0.15 param set-default CA_ROTOR1_TILT 2 -param set-default CA_ROTOR2_PX 0.15 -param set-default CA_ROTOR2_PY -0.15 -param set-default CA_ROTOR2_KM -0.05 param set-default CA_ROTOR2_TILT 3 -param set-default CA_ROTOR3_PX -0.15 -param set-default CA_ROTOR3_PY 0.15 -param set-default CA_ROTOR3_KM -0.05 param set-default CA_ROTOR3_TILT 4 param set-default CA_SV_CS_COUNT 4 param set-default CA_SV_CS0_TYPE 1 param set-default CA_SV_CS0_TRQ_R -0.5 param set-default CA_SV_CS1_TRQ_R 0.5 param set-default CA_SV_CS1_TYPE 2 -param set-default CA_SV_CS2_TRQ_P 1.0 +param set-default CA_SV_CS2_TRQ_P 1 param set-default CA_SV_CS2_TYPE 3 -param set-default CA_SV_CS3_TRQ_Y 1.0 +param set-default CA_SV_CS3_TRQ_Y 1 param set-default CA_SV_CS3_TYPE 4 param set-default CA_SV_TL_COUNT 4 - +# Square quadrotor X PX4 numbering +param set-default CA_ROTOR_COUNT 4 +param set-default CA_ROTOR0_PX 1 +param set-default CA_ROTOR0_PY 1 +param set-default CA_ROTOR1_PX -1 +param set-default CA_ROTOR1_PY -1 +param set-default CA_ROTOR2_PX 1 +param set-default CA_ROTOR2_PY -1 +param set-default CA_ROTOR2_KM -0.05 +param set-default CA_ROTOR3_PX -1 +param set-default CA_ROTOR3_PY 1 +param set-default CA_ROTOR3_KM -0.05 diff --git a/ROMFS/px4fmu_common/init.d/airframes/13100_generic_vtol_tiltrotor b/ROMFS/px4fmu_common/init.d/airframes/13100_generic_vtol_tiltrotor index 19f9a95eb9e0..be1d47979dfc 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13100_generic_vtol_tiltrotor +++ b/ROMFS/px4fmu_common/init.d/airframes/13100_generic_vtol_tiltrotor @@ -12,19 +12,22 @@ . ${R}etc/init.d/rc.vtol_defaults param set-default CA_AIRFRAME 3 +param set-default CA_ROTOR0_TILT 2 +param set-default CA_ROTOR2_TILT 1 + +# Square quadrotor X PX4 numbering param set-default CA_ROTOR_COUNT 4 param set-default CA_ROTOR0_PX 1 param set-default CA_ROTOR0_PY 1 -param set-default CA_ROTOR0_TILT 2 param set-default CA_ROTOR1_PX -1 param set-default CA_ROTOR1_PY -1 param set-default CA_ROTOR2_PX 1 param set-default CA_ROTOR2_PY -1 -param set-default CA_ROTOR2_TILT 1 param set-default CA_ROTOR2_KM -0.05 param set-default CA_ROTOR3_PX -1 param set-default CA_ROTOR3_PY 1 param set-default CA_ROTOR3_KM -0.05 + param set-default CA_SV_CS_COUNT 4 param set-default CA_SV_CS0_TYPE 1 param set-default CA_SV_CS0_TRQ_R -0.5 diff --git a/ROMFS/px4fmu_common/init.d/airframes/14001_generic_mc_with_tilt b/ROMFS/px4fmu_common/init.d/airframes/14001_generic_mc_with_tilt index e675cafe1b95..3eb558c46e78 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/14001_generic_mc_with_tilt +++ b/ROMFS/px4fmu_common/init.d/airframes/14001_generic_mc_with_tilt @@ -20,16 +20,16 @@ param set-default MAV_TYPE 15 param set-default CA_AIRFRAME 8 # Tricopter -param set-default CA_ROTOR0_PX 0.2500 -param set-default CA_ROTOR0_PY 0.4330 -param set-default CA_ROTOR1_PX 0.2500 -param set-default CA_ROTOR1_PY -0.4300 -param set-default CA_ROTOR2_PX -0.5000 -param set-default CA_ROTOR2_PY 0.0000 +param set-default CA_ROTOR0_PX 0.25 +param set-default CA_ROTOR0_PY 0.433 +param set-default CA_ROTOR1_PX 0.25 +param set-default CA_ROTOR1_PY -0.43 +param set-default CA_ROTOR2_PX -0.5 +param set-default CA_ROTOR2_PY 0 param set-default CA_ROTOR2_TILT 1 param set-default CA_ROTOR_COUNT 3 -param set-default CA_SV_TL0_MAXA 45.0000 -param set-default CA_SV_TL0_MINA -45.0000 +param set-default CA_SV_TL0_MAXA 45 +param set-default CA_SV_TL0_MINA -45 param set-default CA_SV_TL0_TD 90 param set-default CA_SV_TL_COUNT 1 diff --git a/ROMFS/px4fmu_common/init.d/airframes/2100_standard_plane b/ROMFS/px4fmu_common/init.d/airframes/2100_standard_plane index cd454eafed46..d872654722a3 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/2100_standard_plane +++ b/ROMFS/px4fmu_common/init.d/airframes/2100_standard_plane @@ -18,7 +18,7 @@ param set-default CA_SV_CS0_TYPE 1 param set-default CA_SV_CS0_TRQ_R -0.5 param set-default CA_SV_CS1_TRQ_R 0.5 param set-default CA_SV_CS1_TYPE 2 -param set-default CA_SV_CS2_TRQ_P 1.0 +param set-default CA_SV_CS2_TRQ_P 1 param set-default CA_SV_CS2_TYPE 3 -param set-default CA_SV_CS3_TRQ_Y 1.0 +param set-default CA_SV_CS3_TRQ_Y 1 param set-default CA_SV_CS3_TYPE 4 diff --git a/ROMFS/px4fmu_common/init.d/airframes/2106_albatross b/ROMFS/px4fmu_common/init.d/airframes/2106_albatross index 3b51c32f61be..feac59e94ef5 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/2106_albatross +++ b/ROMFS/px4fmu_common/init.d/airframes/2106_albatross @@ -35,17 +35,17 @@ param set-default FW_W_RMAX 0 param set-default CA_AIRFRAME 1 param set-default CA_ROTOR_COUNT 1 param set-default CA_SV_CS_COUNT 7 -param set-default CA_SV_CS0_TRQ_R -0.5000 +param set-default CA_SV_CS0_TRQ_R -0.5 param set-default CA_SV_CS0_TYPE 1 #left aileron -param set-default CA_SV_CS1_TRQ_R 0.5000 +param set-default CA_SV_CS1_TRQ_R 0.5 param set-default CA_SV_CS1_TYPE 2 #right aileron -param set-default CA_SV_CS2_TRQ_P 0.5000 -param set-default CA_SV_CS2_TRQ_Y -0.5000 +param set-default CA_SV_CS2_TRQ_P 0.5 +param set-default CA_SV_CS2_TRQ_Y -0.5 param set-default CA_SV_CS2_TYPE 13 #left A-tail -param set-default CA_SV_CS3_TRQ_P 0.5000 -param set-default CA_SV_CS3_TRQ_Y 0.5000 +param set-default CA_SV_CS3_TRQ_P 0.5 +param set-default CA_SV_CS3_TRQ_Y 0.5 param set-default CA_SV_CS3_TYPE 14 #right A-tail -param set-default CA_SV_CS4_TRQ_Y 1.0000 +param set-default CA_SV_CS4_TRQ_Y 1 param set-default CA_SV_CS4_TYPE 12 param set-default CA_SV_CS5_TYPE 9 #left flap param set-default CA_SV_CS6_TYPE 10 #right flap diff --git a/ROMFS/px4fmu_common/init.d/airframes/2507_cloudship b/ROMFS/px4fmu_common/init.d/airframes/2507_cloudship index a458d830ea65..65ecfe25eb0a 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/2507_cloudship +++ b/ROMFS/px4fmu_common/init.d/airframes/2507_cloudship @@ -21,21 +21,21 @@ param set-default CBRK_IO_SAFETY 22027 param set-default CA_AIRFRAME 9 param set-default CA_ROTOR_COUNT 3 -param set-default CA_ROTOR0_AX 1.0000 -param set-default CA_ROTOR0_AZ 0.0000 -param set-default CA_ROTOR0_KM 0.0000 -param set-default CA_ROTOR0_PY 2.0000 -param set-default CA_ROTOR1_AX 1.0000 -param set-default CA_ROTOR1_AZ 0.0000 -param set-default CA_ROTOR1_KM 0.0000 -param set-default CA_ROTOR1_PY -2.0000 -param set-default CA_ROTOR2_AY -1.0000 -param set-default CA_ROTOR2_AZ 0.0000 -param set-default CA_ROTOR2_KM 0.0000 -param set-default CA_ROTOR2_PX -10.0000 +param set-default CA_ROTOR0_AX 1 +param set-default CA_ROTOR0_AZ 0 +param set-default CA_ROTOR0_KM 0 +param set-default CA_ROTOR0_PY 2 +param set-default CA_ROTOR1_AX 1 +param set-default CA_ROTOR1_AZ 0 +param set-default CA_ROTOR1_KM 0 +param set-default CA_ROTOR1_PY -2 +param set-default CA_ROTOR2_AY -1 +param set-default CA_ROTOR2_AZ 0 +param set-default CA_ROTOR2_KM 0 +param set-default CA_ROTOR2_PX -10 param set-default CA_SV_CS_COUNT 1 -param set-default CA_SV_CS0_TRQ_P 1.0000 +param set-default CA_SV_CS0_TRQ_P 1 param set-default CA_R_REV 7 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4001_quad_x b/ROMFS/px4fmu_common/init.d/airframes/4001_quad_x index c3df7a9c58a3..47c11b7ccecf 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4001_quad_x +++ b/ROMFS/px4fmu_common/init.d/airframes/4001_quad_x @@ -10,15 +10,15 @@ . ${R}etc/init.d/rc.mc_defaults +# Square quadrotor X PX4 numbering param set-default CA_ROTOR_COUNT 4 -param set-default CA_ROTOR0_PX 0.15 -param set-default CA_ROTOR0_PY 0.15 -param set-default CA_ROTOR1_PX -0.15 -param set-default CA_ROTOR1_PY -0.15 -param set-default CA_ROTOR2_PX 0.15 -param set-default CA_ROTOR2_PY -0.15 +param set-default CA_ROTOR0_PX 1 +param set-default CA_ROTOR0_PY 1 +param set-default CA_ROTOR1_PX -1 +param set-default CA_ROTOR1_PY -1 +param set-default CA_ROTOR2_PX 1 +param set-default CA_ROTOR2_PY -1 param set-default CA_ROTOR2_KM -0.05 -param set-default CA_ROTOR3_PX -0.15 -param set-default CA_ROTOR3_PY 0.15 +param set-default CA_ROTOR3_PX -1 +param set-default CA_ROTOR3_PY 1 param set-default CA_ROTOR3_KM -0.05 - diff --git a/ROMFS/px4fmu_common/init.d/airframes/4014_s500 b/ROMFS/px4fmu_common/init.d/airframes/4014_s500 index 309e949adb9c..41fc2b7f93a6 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4014_s500 +++ b/ROMFS/px4fmu_common/init.d/airframes/4014_s500 @@ -18,14 +18,15 @@ param set-default MC_PITCHRATE_P 0.18 param set-default MC_ROLLRATE_I 0.15 param set-default MC_PITCHRATE_I 0.15 +# Square quadrotor X PX4 numbering param set-default CA_ROTOR_COUNT 4 -param set-default CA_ROTOR0_PX 0.15 -param set-default CA_ROTOR0_PY 0.15 -param set-default CA_ROTOR1_PX -0.15 -param set-default CA_ROTOR1_PY -0.15 -param set-default CA_ROTOR2_PX 0.15 -param set-default CA_ROTOR2_PY -0.15 +param set-default CA_ROTOR0_PX 1 +param set-default CA_ROTOR0_PY 1 +param set-default CA_ROTOR1_PX -1 +param set-default CA_ROTOR1_PY -1 +param set-default CA_ROTOR2_PX 1 +param set-default CA_ROTOR2_PY -1 param set-default CA_ROTOR2_KM -0.05 -param set-default CA_ROTOR3_PX -0.15 -param set-default CA_ROTOR3_PY 0.15 +param set-default CA_ROTOR3_PX -1 +param set-default CA_ROTOR3_PY 1 param set-default CA_ROTOR3_KM -0.05 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4015_holybro_s500 b/ROMFS/px4fmu_common/init.d/airframes/4015_holybro_s500 index 37cf5bb6f990..e78cbf44243c 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4015_holybro_s500 +++ b/ROMFS/px4fmu_common/init.d/airframes/4015_holybro_s500 @@ -22,14 +22,15 @@ param set-default MC_PITCHRATE_I 0.3 param set-default MC_ROLLRATE_D 0.004 param set-default MC_PITCHRATE_D 0.004 +# Square quadrotor X PX4 numbering param set-default CA_ROTOR_COUNT 4 -param set-default CA_ROTOR0_PX 0.15 -param set-default CA_ROTOR0_PY 0.15 -param set-default CA_ROTOR1_PX -0.15 -param set-default CA_ROTOR1_PY -0.15 -param set-default CA_ROTOR2_PX 0.15 -param set-default CA_ROTOR2_PY -0.15 +param set-default CA_ROTOR0_PX 1 +param set-default CA_ROTOR0_PY 1 +param set-default CA_ROTOR1_PX -1 +param set-default CA_ROTOR1_PY -1 +param set-default CA_ROTOR2_PX 1 +param set-default CA_ROTOR2_PY -1 param set-default CA_ROTOR2_KM -0.05 -param set-default CA_ROTOR3_PX -0.15 -param set-default CA_ROTOR3_PY 0.15 +param set-default CA_ROTOR3_PX -1 +param set-default CA_ROTOR3_PY 1 param set-default CA_ROTOR3_KM -0.05 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision b/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision index d56fcfb939db..77e51bf08e35 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision +++ b/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision @@ -97,18 +97,17 @@ param set-default CBRK_IO_SAFETY 22027 param set-default THR_MDL_FAC 0.3 +# Square quadrotor X PX4 numbering param set-default CA_ROTOR_COUNT 4 -param set-default CA_ROTOR0_PX 0.15 -param set-default CA_ROTOR0_PY 0.15 -param set-default CA_ROTOR0_KM 0.05 -param set-default CA_ROTOR1_PX -0.15 -param set-default CA_ROTOR1_PY -0.15 -param set-default CA_ROTOR1_KM 0.05 -param set-default CA_ROTOR2_PX 0.15 -param set-default CA_ROTOR2_PY -0.15 +param set-default CA_ROTOR0_PX 1 +param set-default CA_ROTOR0_PY 1 +param set-default CA_ROTOR1_PX -1 +param set-default CA_ROTOR1_PY -1 +param set-default CA_ROTOR2_PX 1 +param set-default CA_ROTOR2_PY -1 param set-default CA_ROTOR2_KM -0.05 -param set-default CA_ROTOR3_PX -0.15 -param set-default CA_ROTOR3_PY 0.15 +param set-default CA_ROTOR3_PX -1 +param set-default CA_ROTOR3_PY 1 param set-default CA_ROTOR3_KM -0.05 # Outputs @@ -116,4 +115,3 @@ 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 - diff --git a/ROMFS/px4fmu_common/init.d/airframes/4017_nxp_hovergames b/ROMFS/px4fmu_common/init.d/airframes/4017_nxp_hovergames index 7094a554618b..03adea0bee67 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4017_nxp_hovergames +++ b/ROMFS/px4fmu_common/init.d/airframes/4017_nxp_hovergames @@ -21,14 +21,15 @@ param set-default MC_ROLLRATE_I 0.15 param set-default MC_PITCHRATE_P 0.18 param set-default MC_PITCHRATE_I 0.15 +# Square quadrotor X PX4 numbering param set-default CA_ROTOR_COUNT 4 -param set-default CA_ROTOR0_PX 0.15 -param set-default CA_ROTOR0_PY 0.15 -param set-default CA_ROTOR1_PX -0.15 -param set-default CA_ROTOR1_PY -0.15 -param set-default CA_ROTOR2_PX 0.15 -param set-default CA_ROTOR2_PY -0.15 +param set-default CA_ROTOR0_PX 1 +param set-default CA_ROTOR0_PY 1 +param set-default CA_ROTOR1_PX -1 +param set-default CA_ROTOR1_PY -1 +param set-default CA_ROTOR2_PX 1 +param set-default CA_ROTOR2_PY -1 param set-default CA_ROTOR2_KM -0.05 -param set-default CA_ROTOR3_PX -0.15 -param set-default CA_ROTOR3_PY 0.15 +param set-default CA_ROTOR3_PX -1 +param set-default CA_ROTOR3_PY 1 param set-default CA_ROTOR3_KM -0.05 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4019_x500_v2 b/ROMFS/px4fmu_common/init.d/airframes/4019_x500_v2 index e9127020c357..4b56f8ae3a54 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4019_x500_v2 +++ b/ROMFS/px4fmu_common/init.d/airframes/4019_x500_v2 @@ -22,19 +22,20 @@ param set-default MC_PITCHRATE_I 0.3 param set-default MC_ROLLRATE_D 0.004 param set-default MC_PITCHRATE_D 0.004 -param set-default CA_ROTOR_COUNT 4 -param set-default CA_ROTOR0_PX 0.25 -param set-default CA_ROTOR0_PY 0.25 -param set-default CA_ROTOR1_PX -0.25 -param set-default CA_ROTOR1_PY -0.25 -param set-default CA_ROTOR2_PX 0.25 -param set-default CA_ROTOR2_PY -0.25 -param set-default CA_ROTOR2_KM -0.05 -param set-default CA_ROTOR3_PX -0.25 -param set-default CA_ROTOR3_PY 0.25 -param set-default CA_ROTOR3_KM -0.05 - 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 + +# Square quadrotor X PX4 numbering +param set-default CA_ROTOR_COUNT 4 +param set-default CA_ROTOR0_PX 1 +param set-default CA_ROTOR0_PY 1 +param set-default CA_ROTOR1_PX -1 +param set-default CA_ROTOR1_PY -1 +param set-default CA_ROTOR2_PX 1 +param set-default CA_ROTOR2_PY -1 +param set-default CA_ROTOR2_KM -0.05 +param set-default CA_ROTOR3_PX -1 +param set-default CA_ROTOR3_PY 1 +param set-default CA_ROTOR3_KM -0.05 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 b/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 index 3d094d231ac7..8ef02418928e 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 +++ b/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 @@ -98,18 +98,17 @@ param set-default CBRK_IO_SAFETY 22027 param set-default THR_MDL_FAC 0.3 +# Square quadrotor X PX4 numbering param set-default CA_ROTOR_COUNT 4 -param set-default CA_ROTOR0_PX 0.15 -param set-default CA_ROTOR0_PY 0.15 -param set-default CA_ROTOR0_KM 0.05 -param set-default CA_ROTOR1_PX -0.15 -param set-default CA_ROTOR1_PY -0.15 -param set-default CA_ROTOR1_KM 0.05 -param set-default CA_ROTOR2_PX 0.15 -param set-default CA_ROTOR2_PY -0.15 +param set-default CA_ROTOR0_PX 1 +param set-default CA_ROTOR0_PY 1 +param set-default CA_ROTOR1_PX -1 +param set-default CA_ROTOR1_PY -1 +param set-default CA_ROTOR2_PX 1 +param set-default CA_ROTOR2_PY -1 param set-default CA_ROTOR2_KM -0.05 -param set-default CA_ROTOR3_PX -0.15 -param set-default CA_ROTOR3_PY 0.15 +param set-default CA_ROTOR3_PX -1 +param set-default CA_ROTOR3_PY 1 param set-default CA_ROTOR3_KM -0.05 # Outputs @@ -117,4 +116,3 @@ 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 - diff --git a/ROMFS/px4fmu_common/init.d/airframes/4041_beta75x b/ROMFS/px4fmu_common/init.d/airframes/4041_beta75x index 9c723c74fd1a..5549255ec452 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4041_beta75x +++ b/ROMFS/px4fmu_common/init.d/airframes/4041_beta75x @@ -46,22 +46,20 @@ param set-default SYS_HAS_MAG 0 param set-default BAT1_N_CELLS 2 +# Square quadrotor X with reverse turn direction param set-default CA_ROTOR_COUNT 4 -param set-default CA_ROTOR0_PX 0.03 -param set-default CA_ROTOR0_PY 0.03 +param set-default CA_ROTOR0_PX 1 +param set-default CA_ROTOR0_PY 1 param set-default CA_ROTOR0_KM -0.05 -param set-default CA_ROTOR1_PX -0.03 -param set-default CA_ROTOR1_PY -0.03 +param set-default CA_ROTOR1_PX -1 +param set-default CA_ROTOR1_PY -1 param set-default CA_ROTOR1_KM -0.05 -param set-default CA_ROTOR2_PX 0.03 -param set-default CA_ROTOR2_PY -0.03 -param set-default CA_ROTOR2_KM 0.05 -param set-default CA_ROTOR3_PX -0.03 -param set-default CA_ROTOR3_PY 0.03 -param set-default CA_ROTOR3_KM 0.05 +param set-default CA_ROTOR2_PX 1 +param set-default CA_ROTOR2_PY -1 +param set-default CA_ROTOR3_PX -1 +param set-default CA_ROTOR3_PY 1 param set-default PWM_MAIN_FUNC1 104 param set-default PWM_MAIN_FUNC2 101 param set-default PWM_MAIN_FUNC3 102 param set-default PWM_MAIN_FUNC4 103 - diff --git a/ROMFS/px4fmu_common/init.d/airframes/4050_generic_250 b/ROMFS/px4fmu_common/init.d/airframes/4050_generic_250 index efc06c3a3a23..d460138cc815 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4050_generic_250 +++ b/ROMFS/px4fmu_common/init.d/airframes/4050_generic_250 @@ -36,16 +36,15 @@ param set-default SDLOG_PROFILE 19 param set-default IMU_DGYRO_CUTOFF 50 param set-default IMU_GYRO_CUTOFF 90 +# Square quadrotor X PX4 numbering param set-default CA_ROTOR_COUNT 4 -param set-default CA_ROTOR0_PX 0.15 -param set-default CA_ROTOR0_PY 0.15 -param set-default CA_ROTOR1_PX -0.15 -param set-default CA_ROTOR1_PY -0.15 -param set-default CA_ROTOR2_PX 0.15 -param set-default CA_ROTOR2_PY -0.15 +param set-default CA_ROTOR0_PX 1 +param set-default CA_ROTOR0_PY 1 +param set-default CA_ROTOR1_PX -1 +param set-default CA_ROTOR1_PY -1 +param set-default CA_ROTOR2_PX 1 +param set-default CA_ROTOR2_PY -1 param set-default CA_ROTOR2_KM -0.05 -param set-default CA_ROTOR3_PX -0.15 -param set-default CA_ROTOR3_PY 0.15 +param set-default CA_ROTOR3_PX -1 +param set-default CA_ROTOR3_PY 1 param set-default CA_ROTOR3_KM -0.05 - - diff --git a/ROMFS/px4fmu_common/init.d/airframes/4052_holybro_qav250 b/ROMFS/px4fmu_common/init.d/airframes/4052_holybro_qav250 index a01c35a3ac85..4dc7f90aea95 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4052_holybro_qav250 +++ b/ROMFS/px4fmu_common/init.d/airframes/4052_holybro_qav250 @@ -46,20 +46,20 @@ param set-default MPC_Z_VEL_I_ACC 1.7 param set-default THR_MDL_FAC 0.3 +# Square quadrotor X PX4 numbering param set-default CA_ROTOR_COUNT 4 -param set-default CA_ROTOR0_PX 0.15 -param set-default CA_ROTOR0_PY 0.15 -param set-default CA_ROTOR1_PX -0.15 -param set-default CA_ROTOR1_PY -0.15 -param set-default CA_ROTOR2_PX 0.15 -param set-default CA_ROTOR2_PY -0.15 +param set-default CA_ROTOR0_PX 1 +param set-default CA_ROTOR0_PY 1 +param set-default CA_ROTOR1_PX -1 +param set-default CA_ROTOR1_PY -1 +param set-default CA_ROTOR2_PX 1 +param set-default CA_ROTOR2_PY -1 param set-default CA_ROTOR2_KM -0.05 -param set-default CA_ROTOR3_PX -0.15 -param set-default CA_ROTOR3_PY 0.15 +param set-default CA_ROTOR3_PX -1 +param set-default CA_ROTOR3_PY 1 param set-default CA_ROTOR3_KM -0.05 param set-default PWM_MAIN_FUNC1 101 param set-default PWM_MAIN_FUNC2 102 param set-default PWM_MAIN_FUNC3 103 param set-default PWM_MAIN_FUNC4 104 - diff --git a/ROMFS/px4fmu_common/init.d/airframes/4053_holybro_kopis2 b/ROMFS/px4fmu_common/init.d/airframes/4053_holybro_kopis2 index 64d14b8d5c40..81e4b26d4837 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4053_holybro_kopis2 +++ b/ROMFS/px4fmu_common/init.d/airframes/4053_holybro_kopis2 @@ -53,16 +53,17 @@ param set-default MC_AIRMODE 1 param set-default EV_TSK_RC_LOSS 1 +# Square quadrotor X PX4 numbering param set-default CA_ROTOR_COUNT 4 -param set-default CA_ROTOR0_PX 0.15 -param set-default CA_ROTOR0_PY 0.15 -param set-default CA_ROTOR1_PX -0.15 -param set-default CA_ROTOR1_PY -0.15 -param set-default CA_ROTOR2_PX 0.15 -param set-default CA_ROTOR2_PY -0.15 +param set-default CA_ROTOR0_PX 1 +param set-default CA_ROTOR0_PY 1 +param set-default CA_ROTOR1_PX -1 +param set-default CA_ROTOR1_PY -1 +param set-default CA_ROTOR2_PX 1 +param set-default CA_ROTOR2_PY -1 param set-default CA_ROTOR2_KM -0.05 -param set-default CA_ROTOR3_PX -0.15 -param set-default CA_ROTOR3_PY 0.15 +param set-default CA_ROTOR3_PX -1 +param set-default CA_ROTOR3_PY 1 param set-default CA_ROTOR3_KM -0.05 param set-default PWM_MAIN_FUNC1 104 @@ -72,5 +73,3 @@ param set-default PWM_MAIN_FUNC4 103 param set-default PWM_MAIN_TIM0 -2 param set-default PWM_MAIN_TIM1 -2 - - diff --git a/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu b/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu index bcfcd10fda0b..d3f48bcc0780 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu +++ b/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu @@ -11,12 +11,11 @@ . ${R}etc/init.d/rc.mc_defaults - # Battery settings param set-default BAT_CRIT_THR 0.20 param set-default BAT_LOW_THR 0.25 -param set-default BAT1_CAPACITY 2800.0 +param set-default BAT1_CAPACITY 2800 param set-default BAT1_N_CELLS 3 param set-default BAT1_R_INTERNAL 0.02 param set-default BAT1_V_CHARGED 4.26 @@ -38,7 +37,7 @@ param set-default COM_RC_LOSS_T 3 # ekf2 param set-default EKF2_TERR_MASK 1 -param set-default EKF2_BARO_NOISE 2.0 +param set-default EKF2_BARO_NOISE 2 param set-default EKF2_BCOEF_X 31.5 param set-default EKF2_BCOEF_Y 25.5 @@ -56,11 +55,11 @@ param set-default EKF2_MAG_NOISE 0.1 param set-default EKF2_MIN_RNG 0.15 param set-default EKF2_OF_DELAY 38 -param set-default EKF2_OF_GATE 2.0 +param set-default EKF2_OF_GATE 2 param set-default EKF2_OF_POS_X -0.035 param set-default EKF2_OF_POS_Z 0.033 param set-default EKF2_OF_MIN_RNG 0.01 -param set-default EKF2_OF_A_HMAX 7.0 +param set-default EKF2_OF_A_HMAX 7 param set-default EKF2_OF_QMIN 30 param set-default EKF2_PCOEF_XN -0.3 @@ -68,18 +67,18 @@ param set-default EKF2_PCOEF_XP -0.4 param set-default EKF2_PCOEF_YN -0.4 param set-default EKF2_PCOEF_YP -0.4 -param set-default EKF2_RNG_A_VMAX 1.0 +param set-default EKF2_RNG_A_VMAX 1 param set-default EKF2_RNG_CTRL 0 param set-default EKF2_RNG_DELAY 55 param set-default EKF2_RNG_POS_X -0.035 -param set-default EKF2_RNG_POS_Y 0.0 +param set-default EKF2_RNG_POS_Y 0 param set-default EKF2_RNG_POS_Z 0.033 -param set-default EKF2_TERR_NOISE 1.0 +param set-default EKF2_TERR_NOISE 1 # Maximum allowed angle velocity in the landed state -param set-default LNDMC_ROT_MAX 40.0 +param set-default LNDMC_ROT_MAX 40 # Maximum vertical velocity allowed in the landed state param set-default LNDMC_Z_VEL_MAX 0.7 @@ -94,36 +93,36 @@ param set-default IMU_GYRO_CUTOFF 65 param set-default MC_PITCHRATE_P 0.075 param set-default MC_PITCHRATE_I 0.1 param set-default MC_PITCHRATE_D 0.0005 -param set-default MC_PITCHRATE_MAX 360.0 -param set-default MC_PITCH_P 8.0 +param set-default MC_PITCHRATE_MAX 360 +param set-default MC_PITCH_P 8 # Roll angle & rate setting param set-default MC_ROLLRATE_P 0.055 param set-default MC_ROLLRATE_I 0.1 param set-default MC_ROLLRATE_D 0.0005 -param set-default MC_ROLLRATE_MAX 360.0 -param set-default MC_ROLL_P 8.0 +param set-default MC_ROLLRATE_MAX 360 +param set-default MC_ROLL_P 8 # Yaw angle & rate setting param set-default MC_YAWRATE_P 0.1 -param set-default MC_YAWRATE_MAX 120.0 +param set-default MC_YAWRATE_MAX 120 param set-default MC_YAW_P 2.5 -param set-default MPC_ACC_DOWN_MAX 2.0 -param set-default MPC_ACC_HOR 3.0 -param set-default MPC_ACC_HOR_MAX 10.0 -param set-default MPC_ACC_UP_MAX 3.0 +param set-default MPC_ACC_DOWN_MAX 2 +param set-default MPC_ACC_HOR 3 +param set-default MPC_ACC_HOR_MAX 10 +param set-default MPC_ACC_UP_MAX 3 param set-default MPC_ALT_MODE 0 param set-default MPC_LAND_SPEED 0.5 param set-default MPC_MAN_TILT_MAX 20 -param set-default MPC_YAWRAUTO_MAX 80.0 +param set-default MPC_YAWRAUTO_MAX 80 param set-default MPC_THR_HOVER 0.54 param set-default MPC_THR_MAX 0.9 param set-default MPC_THR_MIN 0.06 param set-default MPC_TILTMAX_AIR 30 -param set-default MPC_XY_P 1.0 -param set-default MPC_Z_P 2.0 -param set-default MPC_Z_VEL_MAX_DN 2.0 +param set-default MPC_XY_P 1 +param set-default MPC_Z_P 2 +param set-default MPC_Z_VEL_MAX_DN 2 # gimbal configuration param set-default MNT_MODE_IN 0 @@ -150,21 +149,22 @@ param set-default RC1_TRIM 1000 # optical flow param set-default SENS_FLOW_MAXR 7.4 param set-default SENS_FLOW_MINHGT 0.15 -param set-default SENS_FLOW_MAXHGT 5.0 +param set-default SENS_FLOW_MAXHGT 5 # ignore the SD card errors and use normal startup sound set STARTUP_TUNE "1" +# Square quadrotor X PX4 numbering param set-default CA_ROTOR_COUNT 4 -param set-default CA_ROTOR0_PX 0.15 -param set-default CA_ROTOR0_PY 0.15 -param set-default CA_ROTOR1_PX -0.15 -param set-default CA_ROTOR1_PY -0.15 -param set-default CA_ROTOR2_PX 0.15 -param set-default CA_ROTOR2_PY -0.15 +param set-default CA_ROTOR0_PX 1 +param set-default CA_ROTOR0_PY 1 +param set-default CA_ROTOR1_PX -1 +param set-default CA_ROTOR1_PY -1 +param set-default CA_ROTOR2_PX 1 +param set-default CA_ROTOR2_PY -1 param set-default CA_ROTOR2_KM -0.05 -param set-default CA_ROTOR3_PX -0.15 -param set-default CA_ROTOR3_PY 0.15 +param set-default CA_ROTOR3_PX -1 +param set-default CA_ROTOR3_PY 1 param set-default CA_ROTOR3_KM -0.05 param set-default TAP_ESC_FUNC1 101 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4071_ifo b/ROMFS/px4fmu_common/init.d/airframes/4071_ifo index fdb02714c2a5..ce1c3f8f0e54 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4071_ifo +++ b/ROMFS/px4fmu_common/init.d/airframes/4071_ifo @@ -73,23 +73,22 @@ param set-default MAV_1_CONFIG 102 param set-default MAV_1_MODE 0 param set-default SER_TEL2_BAUD 57600 +param set-default PWM_MAIN_TIM0 0 +# Square quadrotor X PX4 numbering param set-default CA_ROTOR_COUNT 4 -param set-default CA_ROTOR0_PX 0.15 -param set-default CA_ROTOR0_PY 0.15 -param set-default CA_ROTOR1_PX -0.15 -param set-default CA_ROTOR1_PY -0.15 -param set-default CA_ROTOR2_PX 0.15 -param set-default CA_ROTOR2_PY -0.15 +param set-default CA_ROTOR0_PX 1 +param set-default CA_ROTOR0_PY 1 +param set-default CA_ROTOR1_PX -1 +param set-default CA_ROTOR1_PY -1 +param set-default CA_ROTOR2_PX 1 +param set-default CA_ROTOR2_PY -1 param set-default CA_ROTOR2_KM -0.05 -param set-default CA_ROTOR3_PX -0.15 -param set-default CA_ROTOR3_PY 0.15 +param set-default CA_ROTOR3_PX -1 +param set-default CA_ROTOR3_PY 1 param set-default CA_ROTOR3_KM -0.05 -param set-default PWM_MAIN_TIM0 0 - 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/4073_ifo-s b/ROMFS/px4fmu_common/init.d/airframes/4073_ifo-s index b87002620520..a544a49e0367 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4073_ifo-s +++ b/ROMFS/px4fmu_common/init.d/airframes/4073_ifo-s @@ -91,22 +91,22 @@ param set-default EKF2_OF_POS_Z -0.07 # Failsafe param set-default COM_LOW_BAT_ACT 3 +param set-default PWM_MAIN_TIM0 -3 + +# Square quadrotor X PX4 numbering param set-default CA_ROTOR_COUNT 4 -param set-default CA_ROTOR0_PX 0.15 -param set-default CA_ROTOR0_PY 0.15 -param set-default CA_ROTOR1_PX -0.15 -param set-default CA_ROTOR1_PY -0.15 -param set-default CA_ROTOR2_PX 0.15 -param set-default CA_ROTOR2_PY -0.15 +param set-default CA_ROTOR0_PX 1 +param set-default CA_ROTOR0_PY 1 +param set-default CA_ROTOR1_PX -1 +param set-default CA_ROTOR1_PY -1 +param set-default CA_ROTOR2_PX 1 +param set-default CA_ROTOR2_PY -1 param set-default CA_ROTOR2_KM -0.05 -param set-default CA_ROTOR3_PX -0.15 -param set-default CA_ROTOR3_PY 0.15 +param set-default CA_ROTOR3_PX -1 +param set-default CA_ROTOR3_PY 1 param set-default CA_ROTOR3_KM -0.05 -param set-default PWM_MAIN_TIM0 -3 - param set-default PWM_MAIN_FUNC1 101 param set-default PWM_MAIN_FUNC2 102 param set-default PWM_MAIN_FUNC3 103 param set-default PWM_MAIN_FUNC4 104 - diff --git a/ROMFS/px4fmu_common/init.d/airframes/4500_clover4 b/ROMFS/px4fmu_common/init.d/airframes/4500_clover4 index cbfbc1c6bae2..22f8d74462be 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4500_clover4 +++ b/ROMFS/px4fmu_common/init.d/airframes/4500_clover4 @@ -27,16 +27,17 @@ param set-default MPC_XY_P 1.1 param set-default MPC_Z_VEL_P_ACC 4.8 param set-default MPC_Z_P 1.2 +# Square quadrotor X PX4 numbering param set-default CA_ROTOR_COUNT 4 -param set-default CA_ROTOR0_PX 0.15 -param set-default CA_ROTOR0_PY 0.15 -param set-default CA_ROTOR1_PX -0.15 -param set-default CA_ROTOR1_PY -0.15 -param set-default CA_ROTOR2_PX 0.15 -param set-default CA_ROTOR2_PY -0.15 +param set-default CA_ROTOR0_PX 1 +param set-default CA_ROTOR0_PY 1 +param set-default CA_ROTOR1_PX -1 +param set-default CA_ROTOR1_PY -1 +param set-default CA_ROTOR2_PX 1 +param set-default CA_ROTOR2_PY -1 param set-default CA_ROTOR2_KM -0.05 -param set-default CA_ROTOR3_PX -0.15 -param set-default CA_ROTOR3_PY 0.15 +param set-default CA_ROTOR3_PX -1 +param set-default CA_ROTOR3_PY 1 param set-default CA_ROTOR3_KM -0.05 param set-default PWM_MAIN_FUNC1 101 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21 b/ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21 index f9bf887f1ed9..37dfc34def96 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21 +++ b/ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21 @@ -17,7 +17,6 @@ # . ${R}etc/init.d/rc.mc_defaults - param set-default SYS_MC_EST_GROUP 2 param set-default SYS_HAS_MAG 0 param set-default EKF2_OF_CTRL 1 @@ -50,22 +49,22 @@ param set-default MPC_MAX_FLOW_HGT 3 param set-default NAV_RCL_ACT 3 -param set-default CA_ROTOR_COUNT 4 -param set-default CA_ROTOR0_PX 0.03 -param set-default CA_ROTOR0_PY 0.03 -param set-default CA_ROTOR1_PX -0.03 -param set-default CA_ROTOR1_PY 0.03 -param set-default CA_ROTOR1_KM -0.05 -param set-default CA_ROTOR2_PX -0.03 -param set-default CA_ROTOR2_PY -0.03 -param set-default CA_ROTOR3_PX 0.03 -param set-default CA_ROTOR3_PY -0.03 -param set-default CA_ROTOR3_KM -0.05 - # Run the motors at 328.125 kHz (recommended) param set-default PWM_MAIN_TIM0 3921 param set-default PWM_MAIN_TIM1 3921 +# Square quadrotor X clockwise numbering +param set-default CA_ROTOR0_PX 1 +param set-default CA_ROTOR0_PY 1 +param set-default CA_ROTOR1_PX -1 +param set-default CA_ROTOR1_PY 1 +param set-default CA_ROTOR1_KM -0.05 +param set-default CA_ROTOR2_PX -1 +param set-default CA_ROTOR2_PY -1 +param set-default CA_ROTOR3_PX 1 +param set-default CA_ROTOR3_PY -1 +param set-default CA_ROTOR3_KM -0.05 + param set-default PWM_MAIN_FUNC1 101 param set-default PWM_MAIN_FUNC2 102 param set-default PWM_MAIN_FUNC3 103 diff --git a/ROMFS/px4fmu_common/init.d/airframes/50004_nxpcup_car_dfrobot_gpx b/ROMFS/px4fmu_common/init.d/airframes/50004_nxpcup_car_dfrobot_gpx index 6d4bd2f4f569..67620ddc0375 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50004_nxpcup_car_dfrobot_gpx +++ b/ROMFS/px4fmu_common/init.d/airframes/50004_nxpcup_car_dfrobot_gpx @@ -36,9 +36,9 @@ param set-default GND_THR_MAX 0.5 param set-default GND_MAX_ANG 1.042 param set-default GND_WHEEL_BASE 0.17 -# TODO: Set to -1.0, to allow reversing. This will require many changes in the codebase +# TODO: Set to -1, to allow reversing. This will require many changes in the codebase # to support negative throttle. -param set-default GND_THR_MIN 0.0 +param set-default GND_THR_MIN 0 param set-default GND_SPEED_P 0.25 param set-default GND_SPEED_I 3 param set-default GND_SPEED_D 0.001 diff --git a/ROMFS/px4fmu_common/init.d/airframes/5001_quad_+ b/ROMFS/px4fmu_common/init.d/airframes/5001_quad_+ index d68331179223..4f1f578ad170 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/5001_quad_+ +++ b/ROMFS/px4fmu_common/init.d/airframes/5001_quad_+ @@ -1,12 +1,10 @@ #!/bin/sh # -# @name Generic 10" Quad + geometry +# @name Generic Quad + geometry # # @type Quadrotor + # @class Copter # -# @maintainer Lorenz Meier -# # @board bitcraze_crazyflie exclude # @board px4_fmu-v2 exclude # @@ -14,13 +12,17 @@ . ${R}etc/init.d/rc.mc_defaults param set-default CA_ROTOR_COUNT 4 -param set-default CA_ROTOR0_PX 0.15 -param set-default CA_ROTOR0_PY 0.15 -param set-default CA_ROTOR0_KM -0.05 -param set-default CA_ROTOR1_PX -0.15 -param set-default CA_ROTOR1_PY -0.15 -param set-default CA_ROTOR1_KM -0.05 -param set-default CA_ROTOR2_PX 0.15 -param set-default CA_ROTOR2_PY -0.15 -param set-default CA_ROTOR3_PX -0.15 -param set-default CA_ROTOR3_PY 0.15 + +param set-default CA_ROTOR0_PX 0 +param set-default CA_ROTOR0_PY 1 + +param set-default CA_ROTOR1_PX 0 +param set-default CA_ROTOR1_PY -1 + +param set-default CA_ROTOR2_PX 1 +param set-default CA_ROTOR2_PY 0 +param set-default CA_ROTOR2_KM -0.05 + +param set-default CA_ROTOR3_PX -1 +param set-default CA_ROTOR3_PY 0 +param set-default CA_ROTOR3_KM -0.05 diff --git a/ROMFS/px4fmu_common/init.d/airframes/60000_uuv_generic b/ROMFS/px4fmu_common/init.d/airframes/60000_uuv_generic index d6f84f0d44f0..3a9ceaa5d95f 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/60000_uuv_generic +++ b/ROMFS/px4fmu_common/init.d/airframes/60000_uuv_generic @@ -5,8 +5,6 @@ # @type Underwater Robot # @class Underwater Robot # -# @maintainer -# # @board px4_fmu-v2 exclude # @board bitcraze_crazyflie exclude # diff --git a/ROMFS/px4fmu_common/init.d/airframes/60001_uuv_hippocampus b/ROMFS/px4fmu_common/init.d/airframes/60001_uuv_hippocampus index 672d411bc319..a94d0bd00e74 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/60001_uuv_hippocampus +++ b/ROMFS/px4fmu_common/init.d/airframes/60001_uuv_hippocampus @@ -12,38 +12,7 @@ # . ${R}etc/init.d/rc.uuv_defaults - -param set-default CA_AIRFRAME 7 -param set-default CA_ROTOR_COUNT 4 -param set-default CA_R_REV 255 -param set-default CA_ROTOR0_AX 1 -param set-default CA_ROTOR0_AY 0 -param set-default CA_ROTOR0_AZ 0 -param set-default CA_ROTOR0_KM 0 -param set-default CA_ROTOR0_PX 0 -param set-default CA_ROTOR0_PY -0.3 -param set-default CA_ROTOR0_PZ -0.3 -param set-default CA_ROTOR1_AX 1 -param set-default CA_ROTOR1_AY 0 -param set-default CA_ROTOR1_AZ 0 -param set-default CA_ROTOR1_KM 0 -param set-default CA_ROTOR1_PX 0 -param set-default CA_ROTOR1_PY 0.3 -param set-default CA_ROTOR1_PZ -0.3 -param set-default CA_ROTOR2_AX 1 -param set-default CA_ROTOR2_AY 0 -param set-default CA_ROTOR2_AZ 0 -param set-default CA_ROTOR2_KM 0 -param set-default CA_ROTOR2_PX 0 -param set-default CA_ROTOR2_PY 0.3 -param set-default CA_ROTOR2_PZ 0.3 -param set-default CA_ROTOR3_AX 1 -param set-default CA_ROTOR3_AY 0 -param set-default CA_ROTOR3_AZ 0 -param set-default CA_ROTOR3_KM 0 -param set-default CA_ROTOR3_PX 0 -param set-default CA_ROTOR3_PY -0.3 -param set-default CA_ROTOR3_PZ 0.3 +. ${R}etc/init.d/airframes/60000_uuv_generic param set-default PWM_MAIN_FUNC1 101 param set-default PWM_MAIN_FUNC2 102 diff --git a/ROMFS/px4fmu_common/init.d/airframes/60002_uuv_bluerov2_heavy b/ROMFS/px4fmu_common/init.d/airframes/60002_uuv_bluerov2_heavy index fb63525159cd..2afdc17eee63 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/60002_uuv_bluerov2_heavy +++ b/ROMFS/px4fmu_common/init.d/airframes/60002_uuv_bluerov2_heavy @@ -30,7 +30,7 @@ param set-default MAV_1_CONFIG 102 param set-default BAT1_A_PER_V 37.8798 param set-default BAT1_CAPACITY 18000 -param set-default BAT1_V_DIV 11.0 +param set-default BAT1_V_DIV 11 param set-default BAT1_N_CELLS 4 param set-default BAT_V_OFFS_CURR 0.33 @@ -38,49 +38,49 @@ param set-default CA_AIRFRAME 7 param set-default CA_ROTOR_COUNT 8 param set-default CA_R_REV 255 -param set-default CA_ROTOR0_AX 1.0000 -param set-default CA_ROTOR0_AY -1.0000 -param set-default CA_ROTOR0_AZ 0.0000 -param set-default CA_ROTOR0_KM 0.0000 -param set-default CA_ROTOR0_PX 0.5000 -param set-default CA_ROTOR0_PY 0.3000 -param set-default CA_ROTOR0_PZ 0.2000 -param set-default CA_ROTOR1_AX 1.0000 -param set-default CA_ROTOR1_AY 1.0000 -param set-default CA_ROTOR1_AZ 0.0000 -param set-default CA_ROTOR1_KM 0.0000 -param set-default CA_ROTOR1_PX 0.5000 -param set-default CA_ROTOR1_PY -0.3000 -param set-default CA_ROTOR1_PZ 0.2000 -param set-default CA_ROTOR2_AX 1.0000 -param set-default CA_ROTOR2_AY 1.0000 -param set-default CA_ROTOR2_AZ 0.0000 -param set-default CA_ROTOR2_KM 0.0000 -param set-default CA_ROTOR2_PX -0.5000 -param set-default CA_ROTOR2_PY 0.3000 -param set-default CA_ROTOR2_PZ 0.2000 -param set-default CA_ROTOR3_AX 1.0000 -param set-default CA_ROTOR3_AY -1.0000 -param set-default CA_ROTOR3_AZ 0.0000 -param set-default CA_ROTOR3_KM 0.0000 -param set-default CA_ROTOR3_PX -0.5000 -param set-default CA_ROTOR3_PY -0.3000 -param set-default CA_ROTOR3_PZ 0.2000 -param set-default CA_ROTOR4_AZ -1.0000 -param set-default CA_ROTOR4_KM 0.0000 -param set-default CA_ROTOR4_PX 0.5000 -param set-default CA_ROTOR4_PY 0.5000 -param set-default CA_ROTOR5_AZ 1.0000 -param set-default CA_ROTOR5_KM 0.0000 -param set-default CA_ROTOR5_PX 0.5000 -param set-default CA_ROTOR5_PY -0.5000 -param set-default CA_ROTOR6_AZ 1.0000 -param set-default CA_ROTOR6_KM 0.0000 -param set-default CA_ROTOR6_PX -0.5000 -param set-default CA_ROTOR6_PY 0.5000 -param set-default CA_ROTOR7_KM 0.0000 -param set-default CA_ROTOR7_PX -0.5000 -param set-default CA_ROTOR7_PY -0.5000 +param set-default CA_ROTOR0_AX 1 +param set-default CA_ROTOR0_AY -1 +param set-default CA_ROTOR0_AZ 0 +param set-default CA_ROTOR0_KM 0 +param set-default CA_ROTOR0_PX 0.5 +param set-default CA_ROTOR0_PY 0.3 +param set-default CA_ROTOR0_PZ 0.2 +param set-default CA_ROTOR1_AX 1 +param set-default CA_ROTOR1_AY 1 +param set-default CA_ROTOR1_AZ 0 +param set-default CA_ROTOR1_KM 0 +param set-default CA_ROTOR1_PX 0.5 +param set-default CA_ROTOR1_PY -0.3 +param set-default CA_ROTOR1_PZ 0.2 +param set-default CA_ROTOR2_AX 1 +param set-default CA_ROTOR2_AY 1 +param set-default CA_ROTOR2_AZ 0 +param set-default CA_ROTOR2_KM 0 +param set-default CA_ROTOR2_PX -0.5 +param set-default CA_ROTOR2_PY 0.3 +param set-default CA_ROTOR2_PZ 0.2 +param set-default CA_ROTOR3_AX 1 +param set-default CA_ROTOR3_AY -1 +param set-default CA_ROTOR3_AZ 0 +param set-default CA_ROTOR3_KM 0 +param set-default CA_ROTOR3_PX -0.5 +param set-default CA_ROTOR3_PY -0.3 +param set-default CA_ROTOR3_PZ 0.2 +param set-default CA_ROTOR4_AZ -1 +param set-default CA_ROTOR4_KM 0 +param set-default CA_ROTOR4_PX 0.5 +param set-default CA_ROTOR4_PY 0.5 +param set-default CA_ROTOR5_AZ 1 +param set-default CA_ROTOR5_KM 0 +param set-default CA_ROTOR5_PX 0.5 +param set-default CA_ROTOR5_PY -0.5 +param set-default CA_ROTOR6_AZ 1 +param set-default CA_ROTOR6_KM 0 +param set-default CA_ROTOR6_PX -0.5 +param set-default CA_ROTOR6_PY 0.5 +param set-default CA_ROTOR7_KM 0 +param set-default CA_ROTOR7_PX -0.5 +param set-default CA_ROTOR7_PY -0.5 param set-default PWM_MAIN_FUNC1 101 param set-default PWM_MAIN_FUNC2 102 diff --git a/ROMFS/px4fmu_common/init.d/airframes/6001_hexa_x b/ROMFS/px4fmu_common/init.d/airframes/6001_hexa_x index 73c2f5ca4274..ec8720d1dc34 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/6001_hexa_x +++ b/ROMFS/px4fmu_common/init.d/airframes/6001_hexa_x @@ -16,10 +16,10 @@ param set-default MAV_TYPE 13 param set-default CA_ROTOR_COUNT 6 -param set-default CA_ROTOR0_PX 0.0 +param set-default CA_ROTOR0_PX 0 param set-default CA_ROTOR0_PY 0.5 param set-default CA_ROTOR0_KM -0.05 -param set-default CA_ROTOR1_PX 0.0 +param set-default CA_ROTOR1_PX 0 param set-default CA_ROTOR1_PY -0.5 param set-default CA_ROTOR2_PX 0.43 param set-default CA_ROTOR2_PY -0.25 diff --git a/ROMFS/px4fmu_common/init.d/airframes/6002_draco_r b/ROMFS/px4fmu_common/init.d/airframes/6002_draco_r index ec3b5c608960..e0eefa351bd8 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/6002_draco_r +++ b/ROMFS/px4fmu_common/init.d/airframes/6002_draco_r @@ -97,10 +97,10 @@ param set-default SER_TEL2_BAUD 921600 param set-default CA_ROTOR_COUNT 6 -param set-default CA_ROTOR0_PX 0.0 +param set-default CA_ROTOR0_PX 0 param set-default CA_ROTOR0_PY 0.5 param set-default CA_ROTOR0_KM -0.05 -param set-default CA_ROTOR1_PX 0.0 +param set-default CA_ROTOR1_PX 0 param set-default CA_ROTOR1_PY -0.5 param set-default CA_ROTOR2_PX 0.43 param set-default CA_ROTOR2_PY -0.25 diff --git a/ROMFS/px4fmu_common/init.d/airframes/7001_hexa_+ b/ROMFS/px4fmu_common/init.d/airframes/7001_hexa_+ index 308bdfb1b857..b22afec00ea1 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/7001_hexa_+ +++ b/ROMFS/px4fmu_common/init.d/airframes/7001_hexa_+ @@ -17,10 +17,10 @@ param set-default MAV_TYPE 13 param set-default CA_ROTOR_COUNT 6 param set-default CA_ROTOR0_PX 0.5 -param set-default CA_ROTOR0_PY 0.0 +param set-default CA_ROTOR0_PY 0 param set-default CA_ROTOR0_KM -0.05 param set-default CA_ROTOR1_PX -0.5 -param set-default CA_ROTOR1_PY 0.0 +param set-default CA_ROTOR1_PY 0 param set-default CA_ROTOR2_PX -0.25 param set-default CA_ROTOR2_PY -0.43 param set-default CA_ROTOR2_KM -0.05 diff --git a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt index 8fbccdee547c..66e29e32b4dd 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt @@ -32,94 +32,127 @@ ############################################################################ px4_add_romfs_files( - # [0-999] Reserved (historical)" - - # [1000, 1999] Simulation setups" - 1001_rc_quad_x.hil - 1002_standard_vtol.hil - 1100_rc_quad_x_sih.hil - 1101_rc_plane_sih.hil - 1102_tailsitter_duo_sih.hil - - # [2000, 2999] Standard planes" - 2100_standard_plane - 2106_albatross - - 2507_cloudship - - # [3000, 3999] Flying wing" - 3000_generic_wing - - # [4000, 4999] Quadrotor x" - 4001_quad_x - 4014_s500 - 4015_holybro_s500 - 4016_holybro_px4vision - 4017_nxp_hovergames - 4019_x500_v2 - 4020_holybro_px4vision_v1_5 - 4041_beta75x - 4050_generic_250 - 4052_holybro_qav250 - 4053_holybro_kopis2 - 4061_atl_mantis_edu - 4071_ifo - 4073_ifo-s - 4500_clover4 - 4901_crazyflie21 - - # [5000, 5999] Quadrotor +" - 5001_quad_+ - - # [6000, 6999] Hexarotor x" - 6001_hexa_x - 6002_draco_r - - # [7000, 7999] Hexarotor +" - 7001_hexa_+ - - # [8000, 8999] Octorotor +" - 8001_octo_x - - # [9000, 9999] Octorotor +" - 9001_octo_+ - - # [11000, 11999] Hexa Cox - 11001_hexa_cox - - # [12000, 12999] Octo Cox - 12001_octo_cox - - # [13000, 13999] VTOL - 13000_generic_vtol_standard - 13100_generic_vtol_tiltrotor - 13013_deltaquad - 13014_vtol_babyshark - 13030_generic_vtol_quad_tiltrotor - 13200_generic_vtol_tailsitter - - # [14000, 14999] MC with tilt - 14001_generic_mc_with_tilt - - 16001_helicopter - - # [17000, 17999] Autogyro - 17002_TF-AutoG2 - 17003_TF-G2 + # [0-999] Reserved (historical) # [18000, 18999] High-altitude balloons 18001_TF-B1 # [22000, 22999] Reserve for custom models - - 24001_dodeca_cox - - 50000_generic_ground_vehicle - 50004_nxpcup_car_dfrobot_gpx - 50003_aion_robotics_r1_rover - - # [60000, 61000] (Unmanned) Underwater Robots - 60000_uuv_generic - 60001_uuv_hippocampus - 60002_uuv_bluerov2_heavy ) + +if(CONFIG_MODULES_SIMULATION_PWM_OUT_SIM) + px4_add_romfs_files( + # [1000, 1999] Simulation setups + 1001_rc_quad_x.hil + 1002_standard_vtol.hil + 1100_rc_quad_x_sih.hil + 1101_rc_plane_sih.hil + 1102_tailsitter_duo_sih.hil + ) +endif() + +if(CONFIG_MODULES_MC_RATE_CONTROL) + px4_add_romfs_files( + # [4000, 4999] Quadrotor x + 4001_quad_x + 4014_s500 + 4015_holybro_s500 + 4016_holybro_px4vision + 4017_nxp_hovergames + 4019_x500_v2 + 4020_holybro_px4vision_v1_5 + 4041_beta75x + 4050_generic_250 + 4052_holybro_qav250 + 4053_holybro_kopis2 + 4061_atl_mantis_edu + 4071_ifo + 4073_ifo-s + 4500_clover4 + 4901_crazyflie21 + + # [5000, 5999] Quadrotor + + 5001_quad_+ + + # [6000, 6999] Hexarotor x + 6001_hexa_x + 6002_draco_r + + # [7000, 7999] Hexarotor + + 7001_hexa_+ + + # [8000, 8999] Octorotor + + 8001_octo_x + + # [9000, 9999] Octorotor + + 9001_octo_+ + + # [11000, 11999] Hexa Cox + 11001_hexa_cox + + # [12000, 12999] Octo Cox + 12001_octo_cox + + # [14000, 14999] MC with tilt + 14001_generic_mc_with_tilt + + 16001_helicopter + + 24001_dodeca_cox + ) +endif() + +if(CONFIG_MODULES_FW_RATE_CONTROL) + px4_add_romfs_files( + # [2000, 2999] Standard planes + 2100_standard_plane + 2106_albatross + + # [3000, 3999] Flying wing + 3000_generic_wing + + # [17000, 17999] Autogyro + 17002_TF-AutoG2 + 17003_TF-G2 + ) +endif() + +if(CONFIG_MODULES_AIRSHIP_ATT_CONTROL) + px4_add_romfs_files( + 2507_cloudship + ) +endif() + +if(CONFIG_MODULES_VTOL_ATT_CONTROL) + px4_add_romfs_files( + # [13000, 13999] VTOL + 13000_generic_vtol_standard + 13100_generic_vtol_tiltrotor + 13013_deltaquad + 13014_vtol_babyshark + 13030_generic_vtol_quad_tiltrotor + 13200_generic_vtol_tailsitter + ) +endif() + +if(CONFIG_MODULES_ROVER_POS_CONTROL) + px4_add_romfs_files( + 50000_generic_ground_vehicle + 50004_nxpcup_car_dfrobot_gpx + ) +endif() + +if(CONFIG_MODULES_DIFFERENTIAL_DRIVE) + px4_add_romfs_files( + 50003_aion_robotics_r1_rover + ) +endif() + +if(CONFIG_MODULES_UUV_ATT_CONTROL) + px4_add_romfs_files( + # [60000, 61000] (Unmanned) Underwater Robots + 60000_uuv_generic + 60001_uuv_hippocampus + 60002_uuv_bluerov2_heavy + ) +endif() diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index f63c1901b185..32991e59b98c 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -28,13 +28,12 @@ param set-default EKF2_ARSP_THR 8 param set-default EKF2_FUSE_BETA 1 param set-default EKF2_GPS_CHECK 21 param set-default EKF2_MAG_ACCLIM 0 -param set-default EKF2_MAG_YAWLIM 0 param set-default EKF2_REQ_EPH 10 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 EKF2_REQ_VDRIFT 1 +param set-default EKF2_RNG_QLTY_T 3 param set-default RTL_TYPE 1 param set-default RTL_RETURN_ALT 100 diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_differential_apps b/ROMFS/px4fmu_common/init.d/rc.rover_differential_apps index 31c79ac9801e..848527a9e975 100644 --- a/ROMFS/px4fmu_common/init.d/rc.rover_differential_apps +++ b/ROMFS/px4fmu_common/init.d/rc.rover_differential_apps @@ -5,7 +5,7 @@ ekf2 start & # Start rover differential drive controller. -differential_drive_control start +differential_drive start # Start Land Detector. land_detector start rover diff --git a/ROMFS/px4fmu_common/init.d/rc.thermal_cal b/ROMFS/px4fmu_common/init.d/rc.thermal_cal index 484b9af69fb6..9614c15b1945 100644 --- a/ROMFS/px4fmu_common/init.d/rc.thermal_cal +++ b/ROMFS/px4fmu_common/init.d/rc.thermal_cal @@ -22,6 +22,11 @@ then set TEMP_COMP_START "true" fi +if param compare -s TC_M_ENABLE 1 +then + set TEMP_COMP_START "true" +fi + if [ "x$TEMP_COMP_START" != "x" ] then temperature_compensation start diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults index 5e6d041221dc..b666d59bb862 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults @@ -21,7 +21,7 @@ param set-default MIS_YAW_TMT 10 param set-default EKF2_ARSP_THR 10 param set-default EKF2_FUSE_BETA 1 -param set-default HTE_VXY_THR 2.0 +param set-default HTE_VXY_THR 2 param set-default MIS_TKO_LAND_REQ 2 diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 0e0041223a5d..466fed1f624d 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -170,7 +170,7 @@ else param select-backup $PARAM_BACKUP_FILE fi - if ver hwcmp PX4_FMU_V5X PX4_FMU_V6X ARK_FMU_V6X PX4_FMU_V6XRT + if mft query -q -k MFT -s MFT_ETHERNET -v 1 then netman update -i eth0 fi @@ -439,7 +439,12 @@ else # # Start a thermal calibration if required. # - . ${R}etc/init.d/rc.thermal_cal + set RC_THERMAL_CAL ${R}etc/init.d/rc.thermal_cal + if [ -f ${RC_THERMAL_CAL} ] + then + . ${RC_THERMAL_CAL} + fi + unset RC_THERMAL_CAL # # Start gimbal to control mounts such as gimbals, disabled by default. @@ -500,7 +505,12 @@ else # # Start the logger. # - . ${R}etc/init.d/rc.logging + set RC_LOGGING ${R}etc/init.d/rc.logging + if [ -f ${RC_LOGGING} ] + then + . ${RC_LOGGING} + fi + unset RC_LOGGING # # Set additional parameters and env variables for selected AUTOSTART. diff --git a/Tools/astyle/files_to_check_code_style.sh b/Tools/astyle/files_to_check_code_style.sh index 486eac9464ce..734d6f5323d5 100755 --- a/Tools/astyle/files_to_check_code_style.sh +++ b/Tools/astyle/files_to_check_code_style.sh @@ -30,4 +30,5 @@ exec find boards msg src platforms test \ -path src/lib/cdrstream/cyclonedds -prune -o \ -path src/lib/cdrstream/rosidl -prune -o \ -path src/modules/zenoh/zenoh-pico -prune -o \ + -path boards/modalai/voxl2/libfc-sensor-api -prune -o \ -type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) | grep $PATTERN diff --git a/Tools/msg/templates/uorb/msg.cpp.em b/Tools/msg/templates/uorb/msg.cpp.em index b77c013b91e1..4ffe4c38d14c 100644 --- a/Tools/msg/templates/uorb/msg.cpp.em +++ b/Tools/msg/templates/uorb/msg.cpp.em @@ -73,9 +73,16 @@ struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path) #include #include +@{ +queue_length = 1 +for constant in spec.constants: + if constant.name == 'ORB_QUEUE_LENGTH': + queue_length = constant.val +}@ + @[for topic in topics]@ static_assert(static_cast(ORB_ID::@topic) == @(all_topics.index(topic)), "ORB_ID index mismatch"); -ORB_DEFINE(@topic, struct @uorb_struct, @(struct_size-padding_end_size), @(message_hash)u, static_cast(ORB_ID::@topic)); +ORB_DEFINE(@topic, struct @uorb_struct, @(struct_size-padding_end_size), @(message_hash)u, static_cast(ORB_ID::@topic), @queue_length); @[end for] void print_message(const orb_metadata *meta, const @uorb_struct& message) diff --git a/Tools/simulation/gazebo-classic/sitl_gazebo-classic b/Tools/simulation/gazebo-classic/sitl_gazebo-classic index 33ac87a37676..da7206e05770 160000 --- a/Tools/simulation/gazebo-classic/sitl_gazebo-classic +++ b/Tools/simulation/gazebo-classic/sitl_gazebo-classic @@ -1 +1 @@ -Subproject commit 33ac87a37676fb597de110e926bbf0a080169ffe +Subproject commit da7206e057703cc645770f02437013358b71e1c0 diff --git a/Tools/simulation/gz b/Tools/simulation/gz index c78f7f014171..6b4ed09d1b49 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit c78f7f01417168e8faab7a83ade2129c0d26b39d +Subproject commit 6b4ed09d1b495fbff663f098979cc046df013abd diff --git a/_last_entry b/_last_entry deleted file mode 100644 index e69de29bb2d1..000000000000 diff --git a/appveyor.yml b/appveyor.yml deleted file mode 100644 index e7f57cbb2f94..000000000000 --- a/appveyor.yml +++ /dev/null @@ -1,47 +0,0 @@ -# Build version -version: "{build}" -# do not shallow clone because we want to infer the version from the last tag - -branches: - only: - - master - - beta - - stable - -# Build worker image (build VM template) -image: Visual Studio 2017 - -environment: - matrix: - - PX4_CONFIG: tests # this builds posix in px4_sitl_test folder and runs tests - - PX4_CONFIG: px4_fmu-v5_default - -install: -# if the toolchain wasn't restored from build cache download and install it -- ps: >- - if (-not (Test-Path C:\PX4)) { - Invoke-WebRequest https://s3-us-west-2.amazonaws.com/px4-tools/PX4+Windows+Cygwin+Toolchain/PX4+Windows+Cygwin+Toolchain+0.9.msi -OutFile C:\Toolchain.msi - Start-Process -Wait msiexec -ArgumentList '/I C:\Toolchain.msi /quiet /qn /norestart /log C:\install.log' - } - -# Note: using Start-Process -Wait is important -# because otherwise the install begins but non-blocking and the result cannot be used just after - -build_script: -# FIXME Temporary we need to create the home folder because it's not contained in installer 0.5 and CI fails if it doesn't exist -- if not exist "C:\PX4\home" mkdir C:\PX4\home -# setup the environmental variables to work within the installed cygwin toolchain -- call C:\PX4\toolchain\scripts\setup-environment.bat x -# safe the repopath for switching to it in cygwin bash -- for /f %%i in ('cygpath -u %%CD%%') do set repopath=%%i -# build the make target -- call bash --login -c "cd $repopath && make $PX4_CONFIG" - -# Note: using bash --login is important -# because otherwise certain things (like python; import numpy) do not work - -cache: -# cache the entire toolchain installation folder to avoid -# downloading it from AWS S3 and installing the MSI each time -# it's ~1.8GB > 1GB free limit for build caches -- C:\PX4 -> appveyor.yml diff --git a/boards/ark/can-flow/firmware.prototype b/boards/ark/can-flow/firmware.prototype index a59e1adca629..fc0df258228e 100644 --- a/boards/ark/can-flow/firmware.prototype +++ b/boards/ark/can-flow/firmware.prototype @@ -4,7 +4,7 @@ "description": "Firmware for the ARK flow board", "image": "", "build_time": 0, - "summary": "ARFFLOW", + "summary": "ARKFLOW", "version": "0.1", "image_size": 0, "image_maxsize": 2080768, diff --git a/boards/ark/fmu-v6x/init/rc.board_defaults b/boards/ark/fmu-v6x/init/rc.board_defaults index 87451b0c52ea..52d098101df1 100644 --- a/boards/ark/fmu-v6x/init/rc.board_defaults +++ b/boards/ark/fmu-v6x/init/rc.board_defaults @@ -22,12 +22,12 @@ param set-default SENS_IMU_TEMP 10.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 +if ver hwtypecmp ARKV6X000 then param set-default SENS_TEMP_ID 2818058 fi -if ver hwtypecmp ARKV6X000001 ARKV6X001001 ARKV6X002001 ARKV6X003001 ARKV6X004001 ARKV6X005001 ARKV6X006001 ARKV6X007001 +if ver hwtypecmp ARKV6X001 then param set-default SENS_TEMP_ID 3014666 fi diff --git a/boards/ark/fmu-v6x/init/rc.board_sensors b/boards/ark/fmu-v6x/init/rc.board_sensors index 20f0c228d59b..a3e32486df80 100644 --- a/boards/ark/fmu-v6x/init/rc.board_sensors +++ b/boards/ark/fmu-v6x/init/rc.board_sensors @@ -5,7 +5,7 @@ set HAVE_PM2 yes set HAVE_PM3 yes -if ver hwtypecmp ARKV6X005000 ARKV6X005001 ARKV6X005002 ARKV6X005003 ARKV6X005004 +if mft query -q -k MFT -s MFT_PM2 -v 0 then set HAVE_PM2 no set HAVE_PM3 no @@ -66,7 +66,7 @@ then fi fi -if ver hwtypecmp ARKV6X000000 ARKV6X001000 ARKV6X002000 ARKV6X003000 ARKV6X004000 ARKV6X005000 ARKV6X006000 ARKV6X007000 +if ver hwtypecmp ARKV6X000 then # Internal SPI bus IIM42652 with SPIX measured frequency of 32.051kHz iim42652 -R 3 -s -b 1 -C 32051 start @@ -78,7 +78,7 @@ then icm42688p -R 6 -s -b 3 -C 32051 start fi -if ver hwtypecmp ARKV6X000001 ARKV6X001001 ARKV6X002001 ARKV6X003001 ARKV6X004001 ARKV6X005001 ARKV6X006001 ARKV6X007001 +if ver hwtypecmp ARKV6X001 then # Internal SPI bus IIM42653 with SPIX measured frequency of 32.051kHz #iim42653 -R 3 -s -b 1 -C 32051 start diff --git a/boards/ark/fmu-v6x/nuttx-config/include/board.h b/boards/ark/fmu-v6x/nuttx-config/include/board.h index 333c20905d21..fa82671a826f 100644 --- a/boards/ark/fmu-v6x/nuttx-config/include/board.h +++ b/boards/ark/fmu-v6x/nuttx-config/include/board.h @@ -248,7 +248,7 @@ /* ADC 1 2 3 clock source */ -#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2 +#define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 /* FDCAN 1 2 clock source */ @@ -380,7 +380,7 @@ #define GPIO_UART5_RX GPIO_UART5_RX_3 /* PD2 */ #define GPIO_UART5_TX GPIO_UART5_TX_3 /* PC12 */ // GPIO_UART5_RTS No remap /* PC8 */ -// GPIO_UART5_CTS No remap /* PC9 */ +#define GPIO_UART5_CTS (GPIO_ALT|GPIO_AF8|GPIO_PORTC|GPIO_PIN9|GPIO_PULLDOWN) /* PC9 */ #define GPIO_USART6_RX GPIO_USART6_RX_1 /* PC7 */ #define GPIO_USART6_TX GPIO_USART6_TX_1 /* PC6 */ diff --git a/boards/ark/fmu-v6x/src/CMakeLists.txt b/boards/ark/fmu-v6x/src/CMakeLists.txt index 3135751c666e..78b8222f19d8 100644 --- a/boards/ark/fmu-v6x/src/CMakeLists.txt +++ b/boards/ark/fmu-v6x/src/CMakeLists.txt @@ -55,7 +55,6 @@ else() init.c led.c mtd.cpp - manifest.c sdio.c spi.cpp spix_sync.c diff --git a/boards/ark/fmu-v6x/src/board_config.h b/boards/ark/fmu-v6x/src/board_config.h index 4782fc7efdd8..04b84631c760 100644 --- a/boards/ark/fmu-v6x/src/board_config.h +++ b/boards/ark/fmu-v6x/src/board_config.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2016-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2016-2024 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -204,25 +204,17 @@ #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 BOARD_HAS_HW_SPLIT_VERSIONING #define GPIO_HW_VER_REV_DRIVE /* PG0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN0) #define GPIO_HW_REV_SENSE /* PH4 */ GPIO_ADC3_INP15 #define GPIO_HW_VER_SENSE /* PH3 */ GPIO_ADC3_INP14 #define HW_INFO_INIT_PREFIX "ARKV6X" -#define BOARD_NUM_SPI_CFG_HW_VERSIONS 8 // Rev 0 and Rev 1 +#define BOARD_NUM_SPI_CFG_HW_VERSIONS 2 // Base/FMUM -#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 ARKV6X_0 HW_FMUM_ID(0x0) // ARKV6X, Sensor Set Rev 0 +#define ARKV6X_1 HW_FMUM_ID(0x1) // ARKV6X, Sensor Set Rev 1 #define UAVCAN_NUM_IFACES_RUNTIME 1 diff --git a/boards/ark/fmu-v6x/src/manifest.c b/boards/ark/fmu-v6x/src/manifest.c deleted file mode 100644 index f4e012bfb068..000000000000 --- a/boards/ark/fmu-v6x/src/manifest.c +++ /dev/null @@ -1,216 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2022 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file manifest.c - * - * This module supplies the interface to the manifest of hardware that is - * optional and dependent on the HW REV and HW VER IDs - * - * The manifest allows the system to know whether a hardware option - * say for example the PX4IO is an no-pop option vs it is broken. - * - */ - -/**************************************************************************** - * Included Files - ****************************************************************************/ - -#include -#include - -#include -#include -#include - -#include "systemlib/px4_macros.h" - -/**************************************************************************** - * Pre-Processor Definitions - ****************************************************************************/ - -typedef struct { - uint32_t hw_ver_rev; /* the version and revision */ - const px4_hw_mft_item_t *mft; /* The first entry */ - uint32_t entries; /* the lenght of the list */ -} px4_hw_mft_list_entry_t; - -typedef px4_hw_mft_list_entry_t *px4_hw_mft_list_entry; -#define px4_hw_mft_list_uninitialized (px4_hw_mft_list_entry) -1 - -static const px4_hw_mft_item_t device_unsupported = {0, 0, 0}; - -// List of components on a specific board configuration -// The index of those components is given by the enum (px4_hw_mft_item_id_t) -// declared in board_common.h -static const px4_hw_mft_item_t hw_mft_list_v0600[] = { - { - // PX4_MFT_PX4IO - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, - { - // PX4_MFT_USB - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, - { - // PX4_MFT_CAN2 - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, -}; - -static const px4_hw_mft_item_t hw_mft_list_v0610[] = { - { - // PX4_MFT_PX4IO - .present = 0, - .mandatory = 0, - .connection = px4_hw_con_unknown, - }, - { - // PX4_MFT_USB - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, - { - // PX4_MFT_CAN2 - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, -}; - -static const px4_hw_mft_item_t hw_mft_list_v0640[] = { - { - // PX4_MFT_PX4IO - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, - { - // PX4_MFT_USB - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, - { - // PX4_MFT_CAN2 - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, -}; - -static const px4_hw_mft_item_t hw_mft_list_v0650[] = { - { - // PX4_MFT_PX4IO - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_unknown, - }, - { - // PX4_MFT_USB - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, - { - // PX4_MFT_CAN2 - .present = 0, - .mandatory = 0, - .connection = px4_hw_con_unknown, - }, -}; - - -static px4_hw_mft_list_entry_t mft_lists[] = { -// ver_rev - {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 -}; - -/************************************************************************************ - * 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/ark/fmu-v6x/src/mtd.cpp b/boards/ark/fmu-v6x/src/mtd.cpp index 80d41e0b78a3..6e6004ad30bc 100644 --- a/boards/ark/fmu-v6x/src/mtd.cpp +++ b/boards/ark/fmu-v6x/src/mtd.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2022 PX4 Development Team. All rights reserved. + * Copyright (C) 2024 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -31,6 +31,9 @@ * ****************************************************************************/ +#include +#include + #include #include // KiB BS nB @@ -86,10 +89,16 @@ static const px4_mft_entry_s mtd_mft = { .pmft = (void *) &board_mtd_config, }; +static const px4_mft_entry_s mft_mft = { + .type = MFT, + .pmft = (void *) system_query_manifest, +}; + static const px4_mft_s mft = { - .nmft = 1, + .nmft = 2, .mfts = { - &mtd_mft + &mtd_mft, + &mft_mft, } }; diff --git a/boards/ark/fmu-v6x/src/spi.cpp b/boards/ark/fmu-v6x/src/spi.cpp index f83a5a91cd4b..fc60153efedf 100644 --- a/boards/ark/fmu-v6x/src/spi.cpp +++ b/boards/ark/fmu-v6x/src/spi.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2022 PX4 Development Team. All rights reserved. + * Copyright (C) 2024 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,7 +36,7 @@ #include constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSIONS] = { - initSPIHWVersion(ARKV6X00, { + initSPIFmumID(ARKV6X_0, { 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}), @@ -59,145 +59,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), - 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}), - 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(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, { + initSPIFmumID(ARKV6X_1, { 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}), diff --git a/boards/ark/septentrio-gps/canbootloader.px4board b/boards/ark/septentrio-gps/canbootloader.px4board new file mode 100644 index 000000000000..46917280f6a4 --- /dev/null +++ b/boards/ark/septentrio-gps/canbootloader.px4board @@ -0,0 +1,5 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m4" +CONFIG_BOARD_ROMFSROOT="" +CONFIG_BOARD_CONSTRAINED_MEMORY=y +CONFIG_DRIVERS_BOOTLOADERS=y diff --git a/boards/ark/septentrio-gps/default.px4board b/boards/ark/septentrio-gps/default.px4board new file mode 100644 index 000000000000..b62a95d689d5 --- /dev/null +++ b/boards/ark/septentrio-gps/default.px4board @@ -0,0 +1,38 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m4" +CONFIG_BOARD_ROMFSROOT="cannode" +CONFIG_BOARD_NO_HELP=y +CONFIG_BOARD_CONSTRAINED_MEMORY=y +CONFIG_DRIVERS_BAROMETER_BMP388=y +CONFIG_DRIVERS_BOOTLOADERS=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y +CONFIG_DRIVERS_MAGNETOMETER_BOSCH_BMM150=y +CONFIG_DRIVERS_SAFETY_BUTTON=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_BOARD_UAVCAN_INTERFACES=1 +CONFIG_DRIVERS_UAVCANNODE=y +CONFIG_UAVCANNODE_BEEP_COMMAND=y +CONFIG_UAVCANNODE_GNSS_FIX=y +CONFIG_UAVCANNODE_LIGHTS_COMMAND=y +CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH=y +CONFIG_UAVCANNODE_RTK_DATA=y +CONFIG_UAVCANNODE_SAFETY_BUTTON=y +CONFIG_UAVCANNODE_STATIC_PRESSURE=y +CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y +CONFIG_MODULES_EKF2=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_SENSORS=y +# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set +# CONFIG_SENSORS_VEHICLE_AIR_DATA is not set +# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOP=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/boards/ark/septentrio-gps/firmware.prototype b/boards/ark/septentrio-gps/firmware.prototype new file mode 100644 index 000000000000..a21fe346ffc8 --- /dev/null +++ b/boards/ark/septentrio-gps/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 84, + "magic": "PX4FWv1", + "description": "Firmware for the ARK Septentrio GPS board", + "image": "", + "build_time": 0, + "summary": "ARKSEPTENTRIOGPS", + "version": "0.1", + "image_size": 0, + "image_maxsize": 2080768, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/ark/septentrio-gps/init/rc.board_defaults b/boards/ark/septentrio-gps/init/rc.board_defaults new file mode 100644 index 000000000000..0db1234ce96d --- /dev/null +++ b/boards/ark/septentrio-gps/init/rc.board_defaults @@ -0,0 +1,15 @@ +#!/bin/sh +# +# board specific defaults +#------------------------------------------------------------------------------ + +param set-default CBRK_IO_SAFETY 0 +param set-default CANNODE_SUB_MBD 1 +param set-default CANNODE_SUB_RTCM 1 +param set-default MBE_ENABLE 1 +param set-default SENS_IMU_CLPNOTI 0 + +safety_button start +tone_alarm start + +ekf2 start diff --git a/boards/ark/septentrio-gps/init/rc.board_sensors b/boards/ark/septentrio-gps/init/rc.board_sensors new file mode 100644 index 000000000000..43e4e6c506cc --- /dev/null +++ b/boards/ark/septentrio-gps/init/rc.board_sensors @@ -0,0 +1,11 @@ +#!/bin/sh +# +# board sensors init +#------------------------------------------------------------------------------ +gps start -d /dev/ttyS0 -p sbf + +icm42688p -R 0 -s start + +bmp388 -I -b 1 start + +bmm150 -I -b 1 start diff --git a/boards/ark/septentrio-gps/nuttx-config/canbootloader/defconfig b/boards/ark/septentrio-gps/nuttx-config/canbootloader/defconfig new file mode 100644 index 000000000000..0208af2cac25 --- /dev/null +++ b/boards/ark/septentrio-gps/nuttx-config/canbootloader/defconfig @@ -0,0 +1,57 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/ark/septentrio-gps/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARCH_CHIP_STM32=y +CONFIG_ARCH_CHIP_STM32F412CE=y +CONFIG_ARCH_INTERRUPTSTACK=4096 +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BINFMT_DISABLE=y +CONFIG_BOARDCTL=y +CONFIG_BOARD_LOOPSPERMSEC=16717 +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_TCBINFO=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DISABLE_MOUNTPOINT=y +CONFIG_EXPERIMENTAL=y +CONFIG_FDCLONE_DISABLE=y +CONFIG_FDCLONE_STDIO=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_IDLETHREAD_STACKSIZE=4096 +CONFIG_INIT_STACKSIZE=4096 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_MM_REGIONS=2 +CONFIG_NAME_MAX=0 +CONFIG_NUNGET_CHARS=0 +CONFIG_PREALLOC_TIMERS=0 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAM_SIZE=262144 +CONFIG_RAM_START=0x20010000 +CONFIG_RAW_BINARY=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_DISABLE_BUFFERING=y +CONFIG_STM32_FLASH_CONFIG_G=y +CONFIG_STM32_NOEXT_VECTORS=y +CONFIG_STM32_TIM8=y +CONFIG_TASK_NAME_SIZE=0 +CONFIG_USEC_PER_TICK=1000 diff --git a/boards/ark/septentrio-gps/nuttx-config/include/board.h b/boards/ark/septentrio-gps/nuttx-config/include/board.h new file mode 100644 index 000000000000..526392b92bbd --- /dev/null +++ b/boards/ark/septentrio-gps/nuttx-config/include/board.h @@ -0,0 +1,152 @@ +/************************************************************************************ + * configs/px4fmu/include/board.h + * include/arch/board/board.h + * + * Copyright (C) 2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ************************************************************************************/ +#include "board_dma_map.h" + +#ifndef __ARCH_BOARD_BOARD_H +#define __ARCH_BOARD_BOARD_H + +#include +#ifndef __ASSEMBLY__ +# include +#endif + +#include + +/* HSI - 8 MHz RC factory-trimmed + * LSI - 32 KHz RC + * HSE - 8 MHz Crystal + * LSE - not installed + */ +#define STM32_BOARD_USEHSE 1 +#define STM32_BOARD_XTAL 8000000 +#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL + +#define STM32_HSI_FREQUENCY 16000000ul +#define STM32_LSI_FREQUENCY 32000 + +/* Main PLL Configuration */ +#define STM32_PLLCFG_PLLM RCC_PLLCFG_PLLM(8) +#define STM32_PLLCFG_PLLN RCC_PLLCFG_PLLN(384) +#define STM32_PLLCFG_PLLP RCC_PLLCFG_PLLP_4 +#define STM32_PLLCFG_PLLQ RCC_PLLCFG_PLLQ(8) +#define STM32_PLLCFG_PLLR RCC_PLLCFG_PLLR(2) + +#define STM32_RCC_PLLI2SCFGR_PLLI2SM RCC_PLLI2SCFGR_PLLI2SM(16) +#define STM32_RCC_PLLI2SCFGR_PLLI2SN RCC_PLLI2SCFGR_PLLI2SN(192) +#define STM32_RCC_PLLI2SCFGR_PLLI2SQ RCC_PLLI2SCFGR_PLLI2SQ(2) +#define STM32_RCC_PLLI2SCFGR_PLLI2SR RCC_PLLI2SCFGR_PLLI2SR(2) +#define STM32_RCC_PLLI2SCFGR_PLLI2SSRC RCC_PLLI2SCFGR_PLLI2SSRC(0) /* HSE or HSI depending on PLLSRC of PLLCFGR*/ + +#define STM32_RCC_DCKCFGR2_CK48MSEL RCC_DCKCFGR2_CK48MSEL_PLL +#define STM32_RCC_DCKCFGR2_FMPI2C1SEL RCC_DCKCFGR2_FMPI2C1SEL_APB +#define STM32_RCC_DCKCFGR2_SDIOSEL RCC_DCKCFGR2_SDIOSEL_48MHZ + +#define STM32_SYSCLK_FREQUENCY 96000000ul + +/* AHB clock (HCLK) is SYSCLK (96MHz) */ +#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK /* HCLK = SYSCLK / 1 */ +#define STM32_HCLK_FREQUENCY STM32_SYSCLK_FREQUENCY +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* Same as above, to satisfy compiler */ + +/* APB1 clock (PCLK1) is HCLK/2 (48MHz) */ +#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLKd2 /* PCLK1 = HCLK / 2 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* Timers driven from APB1 will be twice PCLK1 (see page 112 of reference manual) */ +#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_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* APB2 clock (PCLK2) is HCLK (96MHz) */ +#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLK /* PCLK2 = HCLK */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY) + +/* Timers driven from APB2 will be PCLK2 since no prescale division */ +#define STM32_APB2_TIM1_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM9_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM10_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM11_CLKIN (STM32_PCLK2_FREQUENCY) + +/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx otherwise frequency is 2xAPBx. */ +#define BOARD_TIM2_FREQUENCY (2 * STM32_PCLK1_FREQUENCY) +#define BOARD_TIM3_FREQUENCY (2 * STM32_PCLK1_FREQUENCY) +#define BOARD_TIM4_FREQUENCY (2 * STM32_PCLK1_FREQUENCY) +#define BOARD_TIM5_FREQUENCY (2 * STM32_PCLK1_FREQUENCY) +#define BOARD_TIM6_FREQUENCY (2 * STM32_PCLK1_FREQUENCY) +#define BOARD_TIM7_FREQUENCY (2 * STM32_PCLK1_FREQUENCY) +#define BOARD_TIM8_FREQUENCY (2 * STM32_PCLK2_FREQUENCY) + +/* Alternate function pin selections ************************************************/ + +/* UARTs */ +#define GPIO_USART1_RX GPIO_USART1_RX_2 +#define GPIO_USART1_TX GPIO_USART1_TX_3 + +#define GPIO_USART2_RX GPIO_USART2_RX_1 +#define GPIO_USART2_TX GPIO_USART2_TX_1 + +/* CAN */ +#define GPIO_CAN1_RX GPIO_CAN1_RX_1 +#define GPIO_CAN1_TX GPIO_CAN1_TX_1 + +/* I2C */ + +#define GPIO_MCU_I2C1_SCL +#define GPIO_MCU_I2C1_SDA + +#define GPIO_I2C1_SCL GPIO_I2C1_SCL_1 +#define GPIO_I2C1_SDA GPIO_I2C1_SDA_1 + +#define GPIO_I2C1_SCL_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN6) +#define GPIO_I2C1_SDA_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN7) + +#define GPIO_I2C2_SCL GPIO_I2C2_SCL_1 +#define GPIO_I2C2_SDA GPIO_I2C2_SDA_4 + +#define GPIO_I2C2_SCL_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN9) +#define GPIO_I2C2_SDA_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN10) + +/* SPI */ +#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 +#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 +#define GPIO_SPI1_SCK GPIO_SPI1_SCK_1 + +#endif /* __ARCH_BOARD_BOARD_H */ diff --git a/boards/ark/septentrio-gps/nuttx-config/include/board_dma_map.h b/boards/ark/septentrio-gps/nuttx-config/include/board_dma_map.h new file mode 100644 index 000000000000..efa3d824b2f6 --- /dev/null +++ b/boards/ark/septentrio-gps/nuttx-config/include/board_dma_map.h @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * 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 + +// DMA1 Channel/Stream Selections +//--------------------------------------------//---------------------------//---------------- + + +// DMA2 Channel/Stream Selections +//--------------------------------------------//---------------------------//---------------- +#define DMACHAN_SPI1_RX DMAMAP_SPI1_RX_1 // DMA2, Stream 0, Channel 3 +#define DMACHAN_SPI1_TX DMAMAP_SPI1_TX_1 // DMA2, Stream 3, Channel 3 +#define DMACHAN_USART1_RX DMAMAP_USART1_RX_1 // DMA2, Stream 2, Channel 4 +#define DMAMAP_USART1_RX DMAMAP_USART1_RX_1 +//#define DMACHAN_USART1_TX DMAMAP_USART1_TX // DMA2, Stream 7, Channel 4 diff --git a/boards/ark/septentrio-gps/nuttx-config/nsh/defconfig b/boards/ark/septentrio-gps/nuttx-config/nsh/defconfig new file mode 100644 index 000000000000..176ae3eccc5b --- /dev/null +++ b/boards/ark/septentrio-gps/nuttx-config/nsh/defconfig @@ -0,0 +1,153 @@ +# +# 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_NSH_DISABLEBG is not set +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLE_CAT is not set +# CONFIG_NSH_DISABLE_CD is not set +# CONFIG_NSH_DISABLE_CP is not set +# CONFIG_NSH_DISABLE_DATE is not set +# CONFIG_NSH_DISABLE_DF is not set +# CONFIG_NSH_DISABLE_ECHO is not set +# CONFIG_NSH_DISABLE_ENV is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXPORT is not set +# CONFIG_NSH_DISABLE_FREE is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_HELP is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_KILL is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_LS is not set +# CONFIG_NSH_DISABLE_MKDIR is not set +# CONFIG_NSH_DISABLE_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/ark/septentrio-gps/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARCH_CHIP_STM32=y +CONFIG_ARCH_CHIP_STM32F412CE=y +CONFIG_ARCH_INTERRUPTSTACK=768 +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_ASSERT_RESET_VALUE=0 +CONFIG_BOARD_LOOPSPERMSEC=16717 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_TCBINFO=y +CONFIG_DEFAULT_SMALL=y +CONFIG_FDCLONE_STDIO=y +CONFIG_FS_CROMFS=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=2624 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_MAX_EXITFUNS=1 +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MM_REGIONS=2 +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_VARS=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PTHREAD_MUTEX_ROBUST=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAM_SIZE=262144 +CONFIG_RAM_START=0x20000000 +CONFIG_RAW_BINARY=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_WAITPID=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=32 +CONFIG_STM32_ADC1=y +CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y +CONFIG_STM32_DMA1=y +CONFIG_STM32_DMA2=y +CONFIG_STM32_FLASH_CONFIG_G=y +CONFIG_STM32_FLASH_PREFETCH=y +CONFIG_STM32_FLOWCONTROL_BROKEN=y +CONFIG_STM32_I2C1=y +CONFIG_STM32_I2C2=y +CONFIG_STM32_JTAG_SW_ENABLE=y +CONFIG_STM32_PWR=y +CONFIG_STM32_SERIALBRK_BSDCOMPAT=y +CONFIG_STM32_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32_SPI1=y +CONFIG_STM32_SPI1_DMA=y +CONFIG_STM32_SPI1_DMA_BUFFER=1024 +CONFIG_STM32_TIM8=y +CONFIG_STM32_USART1=y +CONFIG_STM32_USART2=y +CONFIG_STM32_USART_BREAKS=y +CONFIG_STM32_WWDG=y +CONFIG_SYSTEM_NSH=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_USART1_BAUD=57600 +CONFIG_USART1_RXBUFSIZE=2000 +CONFIG_USART1_RXDMA=y +CONFIG_USART1_TXBUFSIZE=2000 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_SERIAL_CONSOLE=y +CONFIG_USART2_TXBUFSIZE=1100 +CONFIG_USEC_PER_TICK=1000 diff --git a/boards/ark/septentrio-gps/nuttx-config/scripts/canbootloader_script.ld b/boards/ark/septentrio-gps/nuttx-config/scripts/canbootloader_script.ld new file mode 100644 index 000000000000..48a59fe92d0e --- /dev/null +++ b/boards/ark/septentrio-gps/nuttx-config/scripts/canbootloader_script.ld @@ -0,0 +1,134 @@ +/**************************************************************************** + * nuttx-config/scripts/canbootloader_script.ld + * + * Copyright (C) 2015 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 STM32F412 has 1M of FLASH beginning at address 0x0800:0000 and + * 256Kb of SRAM. SRAM is split up into three blocks: + * + * 1) 112Kb of SRAM beginning at address 0x2000:0000 + * 2) 16Kb of SRAM beginning at address 0x2001:c000 + * 3) 64Kb of SRAM beginning at address 0x2002:0000 + * 4) 64Kb of TCM SRAM beginning at address 0x1000: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 first 0x10000 of flash is reserved for the bootloader. + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x08000000, LENGTH = 32K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K +} + +OUTPUT_ARCH(arm) + +ENTRY(__start) /* treat __start as the anchor for dead code stripping */ +EXTERN(_vectors) /* force the vectors to be included in the output */ + +/* + * 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) +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/boards/ark/septentrio-gps/nuttx-config/scripts/script.ld b/boards/ark/septentrio-gps/nuttx-config/scripts/script.ld new file mode 100644 index 000000000000..2f4769b8f5b4 --- /dev/null +++ b/boards/ark/septentrio-gps/nuttx-config/scripts/script.ld @@ -0,0 +1,146 @@ +/**************************************************************************** + * scripts/ld.script + * + * Copyright (C) 2011 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 STM32F412 has 1M of FLASH beginning at address 0x0800:0000 and + * 256Kb of SRAM. SRAM is split up into three blocks: + * + * 1) 112Kb of SRAM beginning at address 0x2000:0000 + * 2) 16Kb of SRAM beginning at address 0x2001:c000 + * 3) 64Kb of SRAM beginning at address 0x2002:0000 + * 4) 64Kb of TCM SRAM beginning at address 0x1000: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 first 0x10000 of flash is reserved for the bootloader. + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x08010000, LENGTH = 928K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K +} + +OUTPUT_ARCH(arm) + +ENTRY(__start) /* treat __start as the anchor for dead code stripping */ +EXTERN(_vectors) /* force the vectors to be included in the output */ + +/* + * 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(8); + /* + * This section positions the app_descriptor_t used + * by the make_can_boot_descriptor.py tool to set + * the application image's descriptor so that the + * uavcan bootloader has the ability to validate the + * image crc, size etc + */ + KEEP(*(.app_descriptor)) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/boards/ark/septentrio-gps/src/CMakeLists.txt b/boards/ark/septentrio-gps/src/CMakeLists.txt new file mode 100644 index 000000000000..06bd98156f22 --- /dev/null +++ b/boards/ark/septentrio-gps/src/CMakeLists.txt @@ -0,0 +1,67 @@ +############################################################################ +# +# 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. +# +############################################################################ + +if("${PX4_BOARD_LABEL}" STREQUAL "canbootloader") + + add_library(drivers_board + boot_config.h + boot.c + led.c + led.h + ) + target_link_libraries(drivers_board + PRIVATE + nuttx_arch + nuttx_drivers + canbootloader + ) + target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/canbootloader) + +else() + add_library(drivers_board + can.c + i2c.cpp + init.c + led.c + spi.cpp + ) + + target_link_libraries(drivers_board + PRIVATE + arch_spi + drivers__led # drv_led_start + nuttx_arch + nuttx_drivers + px4_layer + ) +endif() diff --git a/boards/ark/septentrio-gps/src/board_config.h b/boards/ark/septentrio-gps/src/board_config.h new file mode 100644 index 000000000000..2263956bae0f --- /dev/null +++ b/boards/ark/septentrio-gps/src/board_config.h @@ -0,0 +1,125 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file board_config.h + * + * board internal definitions + */ + +#pragma once + +#include +#include +#include + +/* BUTTON */ +#define GPIO_BTN_SAFETY /* PB15 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTB|GPIO_PIN15) + +/* Safety LED */ +#define GPIO_LED_SAFETY /* PA1 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN1) + +/* Tone alarm output. */ +#define TONE_ALARM_TIMER 2 /* timer 2 */ +#define TONE_ALARM_CHANNEL 1 /* channel 1 */ +#define GPIO_TONE_ALARM_IDLE /* PA0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0) +#define GPIO_TONE_ALARM /* PA0 */ (GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN0) + +/* CAN Silent mode control */ +#define GPIO_CAN1_SILENT_S0 /* PB12 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN12) + +/* CAN termination software control */ +#define GPIO_CAN1_TERMINATION /* PB13 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN13) +#define GPIO_CAN_TERM GPIO_CAN1_TERMINATION + +/* ICM42688p FSYNC */ +#define GPIO_42688P_FSYNC /* PB8 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8) + +/* Boot config */ +#define GPIO_BOOT_CONFIG /* PC15 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN15|GPIO_EXTI) + +/* LEDs are driven with open drain to support Anode to 5V or 3.3V */ +#define GPIO_TIM1_CH1 /* PA8 */ (GPIO_TIM1_CH1_1|GPIO_OPENDRAIN|GPIO_SPEED_2MHz) +#define GPIO_TIM1_CH2 /* PA9 */ (GPIO_TIM1_CH2_1|GPIO_OPENDRAIN|GPIO_SPEED_2MHz) +#define GPIO_TIM1_CH3 /* PA10 */ (GPIO_TIM1_CH3_1|GPIO_OPENDRAIN|GPIO_SPEED_2MHz) + +#define GPIO_I2C1_SCL_RESET /* PB6 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN6) +#define GPIO_I2C1_SDA_RESET /* PB7 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN7) + +#define GPIO_I2C2_SCL_RESET /* PB10 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) +#define GPIO_I2C2_SDA_RESET /* PB9 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9) + +#define GPIO_I2C2_SCL_RESET /* PB10 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) +#define GPIO_I2C2_SDA_RESET /* PB9 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9) + +#define GPIO_USART1_RX_GPIO (GPIO_INPUT|GPIO_PULLUP|GPIO_SPEED_50MHz|GPIO_PORTB|GPIO_PIN3) +#define GPIO_USART1_TX_GPIO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_PULLUP|GPIO_SPEED_50MHz|GPIO_PORTA|GPIO_PIN15) + +#define GPIO_USART2_RX_GPIO (GPIO_INPUT|GPIO_PULLUP|GPIO_SPEED_50MHz|GPIO_PORTA|GPIO_PIN3) +#define GPIO_USART2_TX_GPIO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_PULLUP|GPIO_SPEED_50MHz|GPIO_PORTA|GPIO_PIN2) + +#define FLASH_BASED_PARAMS + +/* High-resolution timer */ +#define HRT_TIMER 3 /* use timer 3 for the HRT */ +#define HRT_TIMER_CHANNEL 4 /* use capture/compare channel 4 */ + +#define PX4_GPIO_INIT_LIST { \ + GPIO_BTN_SAFETY, \ + GPIO_LED_SAFETY, \ + GPIO_I2C1_SCL_RESET, \ + GPIO_I2C1_SDA_RESET, \ + GPIO_I2C2_SCL_RESET, \ + GPIO_I2C2_SDA_RESET, \ + GPIO_42688P_FSYNC, \ + GPIO_BOOT_CONFIG, \ + GPIO_CAN1_TX, \ + GPIO_CAN1_RX, \ + GPIO_CAN1_SILENT_S0, \ + GPIO_CAN1_TERMINATION, \ + } + +__BEGIN_DECLS + +#define BOARD_HAS_N_S_RGB_LED 1 +#define BOARD_MAX_LEDS BOARD_HAS_N_S_RGB_LED + +#ifndef __ASSEMBLY__ + +extern void stm32_spiinitialize(void); + +#include + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/boards/ark/septentrio-gps/src/boot.c b/boards/ark/septentrio-gps/src/boot.c new file mode 100644 index 000000000000..a26034e254f6 --- /dev/null +++ b/boards/ark/septentrio-gps/src/boot.c @@ -0,0 +1,188 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * Author: Ben Dyer + * Pavel Kirienko + * 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 PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (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 "boot_config.h" +#include "board.h" + +#include +#include +#include + +#include +#include "led.h" + +/************************************************************************************ + * 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) +{ + putreg32(getreg32(STM32_RCC_APB1ENR) | RCC_APB1ENR_CAN1EN, STM32_RCC_APB1ENR); + stm32_configgpio(GPIO_CAN1_RX); + stm32_configgpio(GPIO_CAN1_TX); + stm32_configgpio(GPIO_CAN1_SILENT_S0); + stm32_configgpio(GPIO_CAN1_TERMINATION); + putreg32(getreg32(STM32_RCC_APB1RSTR) | RCC_APB1RSTR_CAN1RST, STM32_RCC_APB1RSTR); + putreg32(getreg32(STM32_RCC_APB1RSTR) & ~RCC_APB1RSTR_CAN1RST, STM32_RCC_APB1RSTR); + +#if defined(OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO) + stm32_configgpio(GPIO_GETNODEINFO_JUMPER); +#endif + +} + +/************************************************************************************ + * Name: board_deinitialize + * + * Description: + * This function is called by the bootloader code prior to booting + * the application. Is should place the HW into an benign initialized state. + * + ************************************************************************************/ + +void board_deinitialize(void) +{ + putreg32(getreg32(STM32_RCC_APB1RSTR) | RCC_APB1RSTR_CAN1RST, STM32_RCC_APB1RSTR); +} + +/**************************************************************************** + * Name: board_get_product_name + * + * Description: + * Called to retrieve the product name. The returned value is a assumed + * to be written to a pascal style string that will be length prefixed + * and not null terminated + * + * Input Parameters: + * product_name - A pointer to a buffer to write the name. + * maxlen - The maximum number of charter that can be written + * + * Returned Value: + * The length of characters written to the buffer. + * + ****************************************************************************/ + +uint8_t board_get_product_name(uint8_t *product_name, size_t maxlen) +{ + DEBUGASSERT(maxlen > UAVCAN_STRLEN(HW_UAVCAN_NAME)); + memcpy(product_name, HW_UAVCAN_NAME, UAVCAN_STRLEN(HW_UAVCAN_NAME)); + return UAVCAN_STRLEN(HW_UAVCAN_NAME); +} + +/**************************************************************************** + * Name: board_get_hardware_version + * + * Description: + * Called to retrieve the hardware version information. The function + * will first initialize the the callers struct to all zeros. + * + * Input Parameters: + * hw_version - A pointer to a uavcan_hardwareversion_t. + * + * Returned Value: + * Length of the unique_id + * + ****************************************************************************/ + +size_t board_get_hardware_version(uavcan_HardwareVersion_t *hw_version) +{ + memset(hw_version, 0, sizeof(uavcan_HardwareVersion_t)); + + hw_version->major = HW_VERSION_MAJOR; + hw_version->minor = HW_VERSION_MINOR; + + return board_get_mfguid(*(mfguid_t *) hw_version->unique_id); +} + +/**************************************************************************** + * Name: board_indicate + * + * Description: + * Provides User feedback to indicate the state of the bootloader + * on board specific hardware. + * + * Input Parameters: + * indication - A member of the uiindication_t + * + * Returned Value: + * None + * + ****************************************************************************/ +#define led(n, code, r , g , b, h) {.red = (r),.green = (g), .blue = (b),.hz = (h)} + +typedef begin_packed_struct struct led_t { + uint8_t red; + uint8_t green; + uint8_t blue; + uint8_t hz; +} end_packed_struct led_t; + +static const led_t i2l[] = { + + led(0, off, 0, 0, 0, 0), + led(1, reset, 128, 128, 128, 30), + led(2, autobaud_start, 0, 128, 0, 1), + led(3, autobaud_end, 0, 128, 0, 2), + led(4, allocation_start, 0, 0, 64, 2), + led(5, allocation_end, 0, 128, 64, 3), + led(6, fw_update_start, 32, 128, 64, 3), + led(7, fw_update_erase_fail, 32, 128, 32, 3), + led(8, fw_update_invalid_response, 64, 0, 0, 1), + led(9, fw_update_timeout, 64, 0, 0, 2), + led(a, fw_update_invalid_crc, 64, 0, 0, 4), + led(b, jump_to_app, 0, 128, 0, 10), + +}; + +void board_indicate(uiindication_t indication) +{ + rgb_led(i2l[indication].red, + i2l[indication].green, + i2l[indication].blue, + i2l[indication].hz); +} diff --git a/boards/ark/septentrio-gps/src/boot_config.h b/boards/ark/septentrio-gps/src/boot_config.h new file mode 100644 index 000000000000..76782f9a93cb --- /dev/null +++ b/boards/ark/septentrio-gps/src/boot_config.h @@ -0,0 +1,130 @@ +/**************************************************************************** + * + * 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 boot_config.h + * + * bootloader definitions that configures the behavior and options + * of the Boot loader + * This file is relies on the parent folder's boot_config.h file and defines + * different usages of the hardware for bootloading + */ + +#pragma once + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +/* Bring in the board_config.h definitions + * todo:make this be pulled in from a targed's build + * files in nuttx*/ + +#include "board_config.h" +#include "uavcan.h" +#include + +#include + +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#define OPT_PREFERRED_NODE_ID ANY_NODE_ID + +//todo:wrap OPT_x in in ifdefs for command line definitions +#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 + +/* + * This Option set is set to 1 ensure a provider of firmware has an + * opportunity update the node's firmware. + * This Option is the default policy and can be overridden by + * a jumper + * When this Policy is set, the node will ignore tboot and + * wait indefinitely for a GetNodeInfo request before booting. + * + * OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT is used to allow + * the polarity of the jumper to be True Active + * + * wait OPT_WAIT_FOR_GETNODEINFO OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO + * Jumper + * yes 1 0 x + * yes 1 1 Active + * no 1 1 Not Active + * no 0 0 X + * yes 0 1 Active + * no 0 1 Not Active + * + */ +#define OPT_WAIT_FOR_GETNODEINFO 0 +#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO 1 +#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT 1 + +#define OPT_ENABLE_WD 1 + +#define OPT_RESTART_TIMEOUT_MS 20000 + +/* Reserved for the Booloader */ +#define OPT_BOOTLOADER_SIZE_IN_K (1024*64) + +/* Reserved for the application out of the total + * system flash minus the BOOTLOADER_SIZE_IN_K + */ +#define OPT_APPLICATION_RESERVER_IN_K 0 + +#define OPT_APPLICATION_IMAGE_OFFSET OPT_BOOTLOADER_SIZE_IN_K +#define OPT_APPLICATION_IMAGE_LENGTH (FLASH_SIZE-(OPT_BOOTLOADER_SIZE_IN_K+OPT_APPLICATION_RESERVER_IN_K)) + + +#define FLASH_BASE STM32_FLASH_BASE +#define FLASH_SIZE STM32_FLASH_SIZE + +#define APPLICATION_LOAD_ADDRESS (FLASH_BASE + OPT_APPLICATION_IMAGE_OFFSET) +#define APPLICATION_SIZE (FLASH_SIZE-OPT_APPLICATION_IMAGE_OFFSET) +#define APPLICATION_LAST_8BIT_ADDRRESS ((uint8_t *)((APPLICATION_LOAD_ADDRESS+APPLICATION_SIZE)-sizeof(uint8_t))) +#define APPLICATION_LAST_32BIT_ADDRRESS ((uint32_t *)((APPLICATION_LOAD_ADDRESS+APPLICATION_SIZE)-sizeof(uint32_t))) +#define APPLICATION_LAST_64BIT_ADDRRESS ((uint64_t *)((APPLICATION_LOAD_ADDRESS+APPLICATION_SIZE)-sizeof(uint64_t))) + +/* If this board uses big flash that have large sectors */ + +#define OPT_USE_YIELD + +/* Bootloader Option***************************************************************** + * + */ +#define GPIO_GETNODEINFO_JUMPER (GPIO_BOOT_CONFIG & ~GPIO_EXTI) diff --git a/boards/ark/septentrio-gps/src/can.c b/boards/ark/septentrio-gps/src/can.c new file mode 100644 index 000000000000..7737965dc6ea --- /dev/null +++ b/boards/ark/septentrio-gps/src/can.c @@ -0,0 +1,130 @@ +/**************************************************************************** + * + * 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 can.c + * + * Board-specific CAN functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include + +#include +#include + +#include "chip.h" +#include "arm_internal.h" + +#include "stm32.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 diff --git a/boards/ark/septentrio-gps/src/i2c.cpp b/boards/ark/septentrio-gps/src/i2c.cpp new file mode 100644 index 000000000000..76486af73317 --- /dev/null +++ b/boards/ark/septentrio-gps/src/i2c.cpp @@ -0,0 +1,39 @@ +/**************************************************************************** + * + * Copyright (C) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusInternal(1), + initI2CBusInternal(2), +}; diff --git a/boards/ark/septentrio-gps/src/init.c b/boards/ark/septentrio-gps/src/init.c new file mode 100644 index 000000000000..61226d675491 --- /dev/null +++ b/boards/ark/septentrio-gps/src/init.c @@ -0,0 +1,168 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file init.c + * + * board specific early startup code. This file implements the + * board_app_initialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialization. + */ + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include "board_config.h" +#include "led.h" +#include + +#include + +#include +#include +#include + +#include + +#include +#include + +# if defined(FLASH_BASED_PARAMS) +# include +#endif + +/************************************************************************************ + * 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) +{ + watchdog_init(); + + /* configure pins */ + const uint32_t gpio[] = PX4_GPIO_INIT_LIST; + px4_gpio_init(gpio, arraySize(gpio)); + + // Configure SPI all interfaces GPIO & enable power. + stm32_spiinitialize(); + + // Check if button is held. If so go into gps passthrough mode + if (stm32_gpioread(GPIO_BTN_SAFETY)) { + rgb_led(128, 128, 128, 10); + stm32_configgpio(GPIO_USART1_TX_GPIO); + stm32_configgpio(GPIO_USART1_RX_GPIO); + stm32_configgpio(GPIO_USART2_TX_GPIO); + stm32_configgpio(GPIO_USART2_RX_GPIO); + + while (1) { + watchdog_pet(); + stm32_gpiowrite(GPIO_USART2_TX_GPIO, stm32_gpioread(GPIO_USART1_RX_GPIO)); + stm32_gpiowrite(GPIO_USART1_TX_GPIO, stm32_gpioread(GPIO_USART2_RX_GPIO)); + } + } +} + +/**************************************************************************** + * 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) +{ + px4_platform_init(); + +#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 + +#if defined(FLASH_BASED_PARAMS) + static sector_descriptor_t params_sector_map[] = { + {2, 16 * 1024, 0x08008000}, + {3, 16 * 1024, 0x0800C000}, + {0, 0, 0}, + }; + + /* Initialize the flashfs layer to use heap allocated memory */ + int result = parameter_flashfs_init(params_sector_map, NULL, 0); + + if (result != OK) { + syslog(LOG_ERR, "[boot] FAILED to init params in FLASH %d\n", result); + } + +#endif // FLASH_BASED_PARAMS + + /* Configure the HW based on the manifest */ + //px4_platform_configure(); + + return OK; +} diff --git a/boards/ark/septentrio-gps/src/led.c b/boards/ark/septentrio-gps/src/led.c new file mode 100644 index 000000000000..9a80cae08923 --- /dev/null +++ b/boards/ark/septentrio-gps/src/led.c @@ -0,0 +1,124 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file led.c + * + * LED backend. + */ + +#include + +#include + +#include "chip.h" +#include "stm32_gpio.h" +#include "board_config.h" + +#include +#include + +#include "led.h" + +#define TMR_BASE STM32_TIM1_BASE +#define TMR_FREQUENCY STM32_APB2_TIM1_CLKIN +#define TMR_REG(o) (TMR_BASE+(o)) + +void rgb_led(int r, int g, int b, int freqs) +{ + + long fosc = TMR_FREQUENCY; + long prescale = 2048; + long p1s = fosc / prescale; + long p0p5s = p1s / 2; + uint16_t val; + static bool once = 0; + + if (!once) { + once = 1; + + /* Enabel Clock to Block */ + modifyreg32(STM32_RCC_APB2ENR, 0, RCC_APB2ENR_TIM1EN); + + /* Reload */ + + val = getreg16(TMR_REG(STM32_BTIM_EGR_OFFSET)); + val |= ATIM_EGR_UG; + putreg16(val, TMR_REG(STM32_BTIM_EGR_OFFSET)); + + /* Set Prescaler STM32_TIM_SETCLOCK */ + + putreg16(prescale, TMR_REG(STM32_BTIM_PSC_OFFSET)); + + /* Enable STM32_TIM_SETMODE*/ + + putreg16(ATIM_CR1_CEN | ATIM_CR1_ARPE, TMR_REG(STM32_BTIM_CR1_OFFSET)); + + + putreg16((ATIM_CCMR_MODE_PWM1 << ATIM_CCMR1_OC1M_SHIFT) | ATIM_CCMR1_OC1PE | + (ATIM_CCMR_MODE_PWM1 << ATIM_CCMR1_OC2M_SHIFT) | ATIM_CCMR1_OC2PE, TMR_REG(STM32_GTIM_CCMR1_OFFSET)); + putreg16((ATIM_CCMR_MODE_PWM1 << ATIM_CCMR2_OC3M_SHIFT) | ATIM_CCMR2_OC3PE, TMR_REG(STM32_GTIM_CCMR2_OFFSET)); + putreg16(ATIM_CCER_CC3E | ATIM_CCER_CC3P | + ATIM_CCER_CC2E | ATIM_CCER_CC2P | + ATIM_CCER_CC1E | ATIM_CCER_CC1P, TMR_REG(STM32_GTIM_CCER_OFFSET)); + + + stm32_configgpio(GPIO_TIM1_CH1); + stm32_configgpio(GPIO_TIM1_CH2); + stm32_configgpio(GPIO_TIM1_CH3); + + /* master output enable = on */ + putreg16(ATIM_BDTR_MOE, (TMR_REG(STM32_ATIM_BDTR_OFFSET))); + } + + long p = freqs == 0 ? p1s : p1s / freqs; + putreg32(p, TMR_REG(STM32_BTIM_ARR_OFFSET)); + + p = freqs == 0 ? p1s + 1 : p0p5s / freqs; + + putreg32((r * p) / 255, TMR_REG(STM32_GTIM_CCR1_OFFSET)); + putreg32((g * p) / 255, TMR_REG(STM32_GTIM_CCR2_OFFSET)); + putreg32((b * p) / 255, TMR_REG(STM32_GTIM_CCR3_OFFSET)); + + val = getreg16(TMR_REG(STM32_BTIM_CR1_OFFSET)); + + if (freqs == 0) { + val &= ~ATIM_CR1_CEN; + + } else { + val |= ATIM_CR1_CEN; + } + + putreg16(val, TMR_REG(STM32_BTIM_CR1_OFFSET)); + +} diff --git a/boards/ark/septentrio-gps/src/led.h b/boards/ark/septentrio-gps/src/led.h new file mode 100644 index 000000000000..b68e4aa70d0c --- /dev/null +++ b/boards/ark/septentrio-gps/src/led.h @@ -0,0 +1,37 @@ +/**************************************************************************** + * + * Copyright (C) 2015 PX4 Development Team. All rights reserved. + * Author: 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 PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +__BEGIN_DECLS +void rgb_led(int r, int g, int b, int freqs); +__END_DECLS diff --git a/boards/ark/septentrio-gps/src/spi.cpp b/boards/ark/septentrio-gps/src/spi.cpp new file mode 100644 index 000000000000..baafb0354c6c --- /dev/null +++ b/boards/ark/septentrio-gps/src/spi.cpp @@ -0,0 +1,44 @@ +/**************************************************************************** + * + * Copyright (C) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include + +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortB, GPIO::Pin0}, SPI::DRDY{GPIO::PortB, GPIO::Pin1}), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); diff --git a/boards/ark/septentrio-gps/uavcan_board_identity b/boards/ark/septentrio-gps/uavcan_board_identity new file mode 100644 index 000000000000..8a80380cf050 --- /dev/null +++ b/boards/ark/septentrio-gps/uavcan_board_identity @@ -0,0 +1,17 @@ +# UAVCAN boot loadable Module ID +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} +) + +set(uavcanblid_hw_version_major 0) +set(uavcanblid_hw_version_minor 84) +set(uavcanblid_name "\"org.ark.septentrio-gps\"") + +add_definitions( + -DHW_UAVCAN_NAME=${uavcanblid_name} + -DHW_VERSION_MAJOR=${uavcanblid_hw_version_major} + -DHW_VERSION_MINOR=${uavcanblid_hw_version_minor} +) diff --git a/boards/atl/mantis-edu/src/board_config.h b/boards/atl/mantis-edu/src/board_config.h index 8b8a99b12e86..f589a6b728f8 100644 --- a/boards/atl/mantis-edu/src/board_config.h +++ b/boards/atl/mantis-edu/src/board_config.h @@ -64,9 +64,6 @@ // Hacks for MAVLink RC button input #define ATL_MANTIS_RC_INPUT_HACKS -// Hacks for MAVLink RC button input -#define ATL_MANTIS_RC_INPUT_HACKS - /* * ADC channels * diff --git a/boards/cuav/nora/nuttx-config/include/board.h b/boards/cuav/nora/nuttx-config/include/board.h index ad4b16fbd518..9aaf12fcdaf4 100644 --- a/boards/cuav/nora/nuttx-config/include/board.h +++ b/boards/cuav/nora/nuttx-config/include/board.h @@ -192,7 +192,7 @@ #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_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 /* ADC 1 2 3 clock source */ #define STM32_RCC_D2CCIP1R_FDCANSEL RCC_D2CCIP1R_FDCANSEL_HSE /* FDCAN 1 2 clock source */ #define STM32_FDCANCLK STM32_HSE_FREQUENCY diff --git a/boards/cuav/x7pro/nuttx-config/include/board.h b/boards/cuav/x7pro/nuttx-config/include/board.h index ad4b16fbd518..9aaf12fcdaf4 100644 --- a/boards/cuav/x7pro/nuttx-config/include/board.h +++ b/boards/cuav/x7pro/nuttx-config/include/board.h @@ -192,7 +192,7 @@ #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_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 /* ADC 1 2 3 clock source */ #define STM32_RCC_D2CCIP1R_FDCANSEL RCC_D2CCIP1R_FDCANSEL_HSE /* FDCAN 1 2 clock source */ #define STM32_FDCANCLK STM32_HSE_FREQUENCY diff --git a/boards/cubepilot/cubeorange/nuttx-config/include/board.h b/boards/cubepilot/cubeorange/nuttx-config/include/board.h index 3a4abd0235e8..5827b1bc81d1 100644 --- a/boards/cubepilot/cubeorange/nuttx-config/include/board.h +++ b/boards/cubepilot/cubeorange/nuttx-config/include/board.h @@ -192,7 +192,7 @@ #define STM32_RCC_D2CCIP2R_USBSRC RCC_D2CCIP2R_USBSEL_PLL3 /* USB 1 and 2 clock source */ #define STM32_RCC_D2CCIP1R_FDCANSEL RCC_D2CCIP1R_FDCANSEL_HSE /* FDCAN 1 2 clock source */ -#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2 /* ADC 1 2 3 clock source */ +#define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 /* ADC 1 2 3 clock source */ /* FLASH wait states */ #define BOARD_FLASH_WAITSTATES 2 diff --git a/boards/cubepilot/cubeorangeplus/nuttx-config/include/board.h b/boards/cubepilot/cubeorangeplus/nuttx-config/include/board.h index 00a886d08601..d650c6169118 100644 --- a/boards/cubepilot/cubeorangeplus/nuttx-config/include/board.h +++ b/boards/cubepilot/cubeorangeplus/nuttx-config/include/board.h @@ -193,7 +193,7 @@ #define STM32_RCC_D2CCIP2R_USBSRC RCC_D2CCIP2R_USBSEL_PLL3 /* USB 1 and 2 clock source */ #define STM32_RCC_D2CCIP1R_FDCANSEL RCC_D2CCIP1R_FDCANSEL_HSE /* FDCAN 1 2 clock source */ -#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2 /* ADC 1 2 3 clock source */ +#define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 /* ADC 1 2 3 clock source */ /* FLASH wait states */ #define BOARD_FLASH_WAITSTATES 2 diff --git a/boards/holybro/durandal-v1/nuttx-config/include/board.h b/boards/holybro/durandal-v1/nuttx-config/include/board.h index 2a475b7b49aa..e1f43bf7b9cd 100644 --- a/boards/holybro/durandal-v1/nuttx-config/include/board.h +++ b/boards/holybro/durandal-v1/nuttx-config/include/board.h @@ -248,7 +248,7 @@ /* ADC 1 2 3 clock source */ -#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2 +#define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 /* FDCAN 1 2 clock source */ diff --git a/boards/holybro/kakuteh7/nuttx-config/include/board.h b/boards/holybro/kakuteh7/nuttx-config/include/board.h index 83245ff925a3..4d3ed5eb4766 100644 --- a/boards/holybro/kakuteh7/nuttx-config/include/board.h +++ b/boards/holybro/kakuteh7/nuttx-config/include/board.h @@ -245,7 +245,7 @@ /* ADC 1 2 3 clock source */ -#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2 +#define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 /* FDCAN 1 2 clock source */ diff --git a/boards/holybro/kakuteh7mini/nuttx-config/include/board.h b/boards/holybro/kakuteh7mini/nuttx-config/include/board.h index 83245ff925a3..4d3ed5eb4766 100644 --- a/boards/holybro/kakuteh7mini/nuttx-config/include/board.h +++ b/boards/holybro/kakuteh7mini/nuttx-config/include/board.h @@ -245,7 +245,7 @@ /* ADC 1 2 3 clock source */ -#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2 +#define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 /* FDCAN 1 2 clock source */ diff --git a/boards/holybro/kakuteh7v2/nuttx-config/include/board.h b/boards/holybro/kakuteh7v2/nuttx-config/include/board.h index 83245ff925a3..4d3ed5eb4766 100644 --- a/boards/holybro/kakuteh7v2/nuttx-config/include/board.h +++ b/boards/holybro/kakuteh7v2/nuttx-config/include/board.h @@ -245,7 +245,7 @@ /* ADC 1 2 3 clock source */ -#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2 +#define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 /* FDCAN 1 2 clock source */ diff --git a/boards/matek/h743-mini/nuttx-config/include/board.h b/boards/matek/h743-mini/nuttx-config/include/board.h index 4bf2b6e45e0f..f8afe3e6b871 100644 --- a/boards/matek/h743-mini/nuttx-config/include/board.h +++ b/boards/matek/h743-mini/nuttx-config/include/board.h @@ -248,7 +248,7 @@ /* ADC 1 2 3 clock source */ -#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2 +#define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 /* FDCAN 1 clock source */ diff --git a/boards/matek/h743-slim/nuttx-config/include/board.h b/boards/matek/h743-slim/nuttx-config/include/board.h index 4bf2b6e45e0f..f8afe3e6b871 100644 --- a/boards/matek/h743-slim/nuttx-config/include/board.h +++ b/boards/matek/h743-slim/nuttx-config/include/board.h @@ -248,7 +248,7 @@ /* ADC 1 2 3 clock source */ -#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2 +#define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 /* FDCAN 1 clock source */ diff --git a/boards/matek/h743/nuttx-config/include/board.h b/boards/matek/h743/nuttx-config/include/board.h index 4bf2b6e45e0f..f8afe3e6b871 100644 --- a/boards/matek/h743/nuttx-config/include/board.h +++ b/boards/matek/h743/nuttx-config/include/board.h @@ -248,7 +248,7 @@ /* ADC 1 2 3 clock source */ -#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2 +#define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 /* FDCAN 1 clock source */ diff --git a/boards/modalai/fc-v1/default.px4board b/boards/modalai/fc-v1/default.px4board index 5999659075b9..2a33d1ddb875 100644 --- a/boards/modalai/fc-v1/default.px4board +++ b/boards/modalai/fc-v1/default.px4board @@ -5,7 +5,7 @@ CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6" CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4" CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1" CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS3" -CONFIG_DRIVERS_ACTUATORS_MODAL_IO=y +CONFIG_DRIVERS_ACTUATORS_VOXL_ESC=y CONFIG_DRIVERS_ADC_ADS1115=y CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_COMMON_BAROMETERS=y diff --git a/boards/modalai/fc-v1/init/rc.board_sensors b/boards/modalai/fc-v1/init/rc.board_sensors index 7268fce687af..03824da992ab 100644 --- a/boards/modalai/fc-v1/init/rc.board_sensors +++ b/boards/modalai/fc-v1/init/rc.board_sensors @@ -3,9 +3,9 @@ # ModalAI FC-v1 specific board sensors init #------------------------------------------------------------------------------ -if param greater MODAL_IO_CONFIG 0 +if param greater VOXL_ESC_CONFIG 0 then - modal_io start + voxl_esc start fi board_adc start diff --git a/boards/modalai/fc-v1/src/board_config.h b/boards/modalai/fc-v1/src/board_config.h index dcc182420ab7..93032bb31fd9 100644 --- a/boards/modalai/fc-v1/src/board_config.h +++ b/boards/modalai/fc-v1/src/board_config.h @@ -264,7 +264,7 @@ #define BOARD_NUM_IO_TIMERS 5 // J4 / TELEM3 / UART2 -#define MODAL_IO_DEFAULT_PORT "/dev/ttyS1" +#define VOXL_ESC_DEFAULT_PORT "/dev/ttyS1" __BEGIN_DECLS diff --git a/boards/modalai/fc-v2/default.px4board b/boards/modalai/fc-v2/default.px4board index a34d1ad7a498..c4e7d3649597 100644 --- a/boards/modalai/fc-v2/default.px4board +++ b/boards/modalai/fc-v2/default.px4board @@ -3,7 +3,7 @@ CONFIG_BOARD_ARCHITECTURE="cortex-m7" CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0" CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6" CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4" -CONFIG_DRIVERS_ACTUATORS_MODAL_IO=y +CONFIG_DRIVERS_ACTUATORS_VOXL_ESC=y CONFIG_DRIVERS_ADC_ADS1115=y CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP201XX=y @@ -26,10 +26,8 @@ CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y CONFIG_DRIVERS_PWM_OUT=y CONFIG_DRIVERS_RC_INPUT=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 @@ -48,6 +46,7 @@ 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 @@ -61,7 +60,7 @@ 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_SIMULATION_SIMULATOR_SIH=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y CONFIG_MODULES_UXRCE_DDS_CLIENT=y CONFIG_MODULES_VTOL_ATT_CONTROL=y diff --git a/boards/modalai/fc-v2/init/rc.board_sensors b/boards/modalai/fc-v2/init/rc.board_sensors index 318a8bc1467b..4fd143d50310 100644 --- a/boards/modalai/fc-v2/init/rc.board_sensors +++ b/boards/modalai/fc-v2/init/rc.board_sensors @@ -3,9 +3,9 @@ # ModalAI FC-v2 specific board sensors init #------------------------------------------------------------------------------ -if param greater MODAL_IO_CONFIG 0 +if param greater VOXL_ESC_CONFIG 0 then - modal_io start + voxl_esc start fi board_adc start diff --git a/boards/modalai/fc-v2/nuttx-config/include/board.h b/boards/modalai/fc-v2/nuttx-config/include/board.h index 34a8d2c2af74..b196e7a68c72 100644 --- a/boards/modalai/fc-v2/nuttx-config/include/board.h +++ b/boards/modalai/fc-v2/nuttx-config/include/board.h @@ -248,7 +248,7 @@ /* ADC 1 2 3 clock source */ -#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2 +#define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 /* FDCAN 1 2 clock source */ diff --git a/boards/modalai/fc-v2/src/board_config.h b/boards/modalai/fc-v2/src/board_config.h index 7e0314cdbce8..987f692b221a 100644 --- a/boards/modalai/fc-v2/src/board_config.h +++ b/boards/modalai/fc-v2/src/board_config.h @@ -351,7 +351,7 @@ #define BOARD_NUM_IO_TIMERS 5 // J5 USART5 TELEM2 Port next to PWM connector -#define MODAL_IO_DEFAULT_PORT "/dev/ttyS4" +#define VOXL_ESC_DEFAULT_PORT "/dev/ttyS4" __BEGIN_DECLS diff --git a/boards/modalai/voxl2-io/default.px4board b/boards/modalai/voxl2-io/default.px4board new file mode 100644 index 000000000000..9e69a1e7460d --- /dev/null +++ b/boards/modalai/voxl2-io/default.px4board @@ -0,0 +1,5 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m3" +CONFIG_BOARD_ROMFSROOT="" +CONFIG_BOARD_CONSTRAINED_FLASH=y +CONFIG_MODULES_PX4IOFIRMWARE=y diff --git a/boards/modalai/voxl2-io/firmware.prototype b/boards/modalai/voxl2-io/firmware.prototype new file mode 100644 index 000000000000..81665310ce5d --- /dev/null +++ b/boards/modalai/voxl2-io/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 41777, + "magic": "PX4FWv2", + "description": "Firmware for the voxl2-io board", + "image": "", + "build_time": 0, + "summary": "voxl2io", + "version": "2.0", + "image_size": 0, + "image_maxsize": 61440, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/modalai/voxl2-io/nuttx-config/include/board.h b/boards/modalai/voxl2-io/nuttx-config/include/board.h new file mode 100644 index 000000000000..936360ab1333 --- /dev/null +++ b/boards/modalai/voxl2-io/nuttx-config/include/board.h @@ -0,0 +1,141 @@ +/************************************************************************************ + * nuttx-configs/px4io/include/board.h + * include/arch/board/board.h + * + * Copyright (C) 2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * 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 NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ************************************************************************************/ + +#ifndef __ARCH_BOARD_BOARD_H +#define __ARCH_BOARD_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include +#ifndef __ASSEMBLY__ +# include +#endif +#include + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ + +/* On-board crystal frequency is 24MHz (HSE) */ + +#define STM32_BOARD_XTAL 16000000ul + +/* Use the HSE output as the system clock */ + +#define STM32_SYSCLK_SW RCC_CFGR_SW_HSE +#define STM32_SYSCLK_SWS RCC_CFGR_SWS_HSE +#define STM32_SYSCLK_FREQUENCY STM32_BOARD_XTAL + +/* AHB clock (HCLK) is SYSCLK (24MHz) */ + +#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK +#define STM32_HCLK_FREQUENCY STM32_SYSCLK_FREQUENCY +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ + +/* APB2 clock (PCLK2) is HCLK (24MHz) */ + +#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLK +#define STM32_PCLK2_FREQUENCY STM32_HCLK_FREQUENCY +#define STM32_APB2_CLKIN (STM32_PCLK2_FREQUENCY) /* Timers 2-4 */ + +/* APB2 timer 1 will receive PCLK2. */ + +#define STM32_APB2_TIM1_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM15_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM16_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM17_CLKIN (STM32_PCLK2_FREQUENCY) + +/* APB1 clock (PCLK1) is HCLK (24MHz) */ + +#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLK +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY) + +/* All timers run off PCLK */ + + +#define STM32_APB1_TIM2_CLKIN (STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM5_CLKIN (STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM6_CLKIN (STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM7_CLKIN (STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM12_CLKIN (STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM13_CLKIN (STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM14_CLKIN (STM32_PCLK1_FREQUENCY) + +/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx + * otherwise frequency is 2xAPBx. + * Note: TIM1, 15-17 are on APB2, others on APB1 + */ + +#define BOARD_TIM1_FREQUENCY STM32_APB2_TIM1_CLKIN +#define BOARD_TIM2_FREQUENCY STM32_APB1_TIM2_CLKIN +#define BOARD_TIM3_FREQUENCY STM32_APB1_TIM3_CLKIN +#define BOARD_TIM4_FREQUENCY STM32_APB1_TIM4_CLKIN +#define BOARD_TIM5_FREQUENCY STM32_APB1_TIM5_CLKIN +#define BOARD_TIM6_FREQUENCY STM32_APB1_TIM6_CLKIN +#define BOARD_TIM7_FREQUENCY STM32_APB1_TIM7_CLKIN +#define BOARD_TIM12_FREQUENCY STM32_APB1_TIM12_CLKIN +#define BOARD_TIM13_FREQUENCY STM32_APB1_TIM13_CLKIN +#define BOARD_TIM14_FREQUENCY STM32_APB1_TIM14_CLKIN +#define BOARD_TIM15_FREQUENCY STM32_APB2_TIM15_CLKIN +#define BOARD_TIM16_FREQUENCY STM32_APB2_TIM16_CLKIN +#define BOARD_TIM17_FREQUENCY STM32_APB2_TIM17_CLKIN + + +/* + * Some of the USART pins are not available; override the GPIO + * definitions with an invalid pin configuration. + */ +#undef GPIO_USART2_CTS +#define GPIO_USART2_CTS 0xffffffff +#undef GPIO_USART2_RTS +#define GPIO_USART2_RTS 0xffffffff +#undef GPIO_USART2_CK +#define GPIO_USART2_CK 0xffffffff +#undef GPIO_USART3_CK +#define GPIO_USART3_CK 0xffffffff +#undef GPIO_USART3_CTS +#define GPIO_USART3_CTS 0xffffffff +#undef GPIO_USART3_RTS +#define GPIO_USART3_RTS 0xffffffff + +#endif /* __ARCH_BOARD_BOARD_H */ diff --git a/boards/modalai/voxl2-io/nuttx-config/nsh/defconfig b/boards/modalai/voxl2-io/nuttx-config/nsh/defconfig new file mode 100644 index 000000000000..2fec96fb2680 --- /dev/null +++ b/boards/modalai/voxl2-io/nuttx-config/nsh/defconfig @@ -0,0 +1,61 @@ +# +# 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_NULL is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/modalai/voxl2-io/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARCH_CHIP_STM32=y +CONFIG_ARCH_CHIP_STM32F100C8=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARM_MPU_EARLY_RESET=y +CONFIG_BOARD_LOOPSPERMSEC=2000 +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DISABLE_MOUNTPOINT=y +CONFIG_FDCLONE_DISABLE=y +CONFIG_FDCLONE_STDIO=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_IDLETHREAD_STACKSIZE=316 +CONFIG_INIT_ENTRYPOINT="user_start" +CONFIG_INIT_STACKSIZE=1100 +CONFIG_MM_FILL_ALLOCATIONS=y +CONFIG_MM_SMALL=y +CONFIG_NAME_MAX=12 +CONFIG_PREALLOC_TIMERS=0 +CONFIG_RAM_SIZE=8192 +CONFIG_RAM_START=0x20000000 +CONFIG_RAW_BINARY=y +CONFIG_SERIAL_TERMIOS=y +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_DISABLE_BUFFERING=y +CONFIG_STM32_ADC1=y +CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y +CONFIG_STM32_DMA1=y +CONFIG_STM32_JTAG_SW_ENABLE=y +CONFIG_STM32_USART1=y +CONFIG_STM32_USART2=y +CONFIG_STM32_USART3=y +CONFIG_STM32_USART_SINGLEWIRE=y +CONFIG_TASK_NAME_SIZE=0 +CONFIG_USART1_RXBUFSIZE=64 +CONFIG_USART1_RXDMA=y +CONFIG_USART1_SERIAL_CONSOLE=y +CONFIG_USART1_TXBUFSIZE=32 +CONFIG_USART2_RXBUFSIZE=64 +CONFIG_USART2_TXBUFSIZE=64 +CONFIG_USART3_RXBUFSIZE=64 +CONFIG_USART3_RXDMA=y +CONFIG_USART3_TXBUFSIZE=64 +CONFIG_USEC_PER_TICK=1000 diff --git a/boards/modalai/voxl2-io/nuttx-config/scripts/script.ld b/boards/modalai/voxl2-io/nuttx-config/scripts/script.ld new file mode 100644 index 000000000000..c84215389d59 --- /dev/null +++ b/boards/modalai/voxl2-io/nuttx-config/scripts/script.ld @@ -0,0 +1,131 @@ +/**************************************************************************** + * configs/px4io-v2/scripts/ld.script + * + * Copyright (C) 2009, 2011 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 STM32F100C8 has 64Kb of FLASH beginning at address 0x0800:0000 and + * 8Kb of SRAM beginning at address 0x2000: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 +{ + flash (rx) : ORIGIN = 0x08001000, LENGTH = 60K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 8K +} + +OUTPUT_ARCH(arm) +ENTRY(__start) /* treat __start as the anchor for dead code stripping */ +EXTERN(_vectors) /* force the vectors to be included in the output */ + +/* + * 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) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + *(.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(.); + + /* The STM32F100CB has 8Kb of SRAM beginning at the following address */ + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + . = ALIGN(4); + } > 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/modalai/voxl2-io/src/CMakeLists.txt b/boards/modalai/voxl2-io/src/CMakeLists.txt new file mode 100644 index 000000000000..5cef2f668c57 --- /dev/null +++ b/boards/modalai/voxl2-io/src/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +add_library(drivers_board + init.c + timer_config.cpp +) + +target_link_libraries(drivers_board + PRIVATE + nuttx_arch +) diff --git a/boards/modalai/voxl2-io/src/board_config.h b/boards/modalai/voxl2-io/src/board_config.h new file mode 100644 index 000000000000..d69aee59b2dc --- /dev/null +++ b/boards/modalai/voxl2-io/src/board_config.h @@ -0,0 +1,157 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (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 + * + * PX4IOV2 internal definitions + */ + +#pragma once + +/****************************************************************************** + * Included Files + ******************************************************************************/ + +#include +#include +#include + +/****************************************************************************** + * Definitions + ******************************************************************************/ +/* Configuration **************************************************************/ + +/****************************************************************************** + * Serial + ******************************************************************************/ +#define PX4FMU_SERIAL_BASE STM32_USART2_BASE +#define PX4FMU_SERIAL_VECTOR STM32_IRQ_USART2 +#define PX4FMU_SERIAL_TX_GPIO GPIO_USART2_TX +#define PX4FMU_SERIAL_RX_GPIO GPIO_USART2_RX +#define PX4FMU_SERIAL_TX_DMA DMACHAN_USART2_TX +#define PX4FMU_SERIAL_RX_DMA DMACHAN_USART2_RX +#define PX4FMU_SERIAL_CLOCK STM32_PCLK1_FREQUENCY +#define PX4FMU_SERIAL_BITRATE 921600 + +/****************************************************************************** + * GPIOS + ******************************************************************************/ + +/* LEDS **********************************************************************/ + +#define GPIO_LED_BLUE (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14) +#define GPIO_LED_AMBER (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15) +#define GPIO_LED_SAFETY (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13) + +#define LED_BLUE(on_true) stm32_gpiowrite(GPIO_LED_BLUE, !(on_true)) +#define LED_AMBER(on_true) stm32_gpiowrite(GPIO_LED_AMBER, !(on_true)) +#define LED_SAFETY(on_true) stm32_gpiowrite(GPIO_LED_SAFETY, !(on_true)) + +//#define GPIO_LED4 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN11) + +//#define GPIO_HEATER_OFF 0 // (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14) + +#define GPIO_PC14 (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTC|GPIO_PIN14) +#define GPIO_PC15 (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTC|GPIO_PIN15) + +#define GPIO_SENSE_PC14_DN (GPIO_INPUT|GPIO_CNF_INPULLDWN|GPIO_MODE_INPUT|GPIO_PORTC|GPIO_PIN14) +#define GPIO_SENSE_PC15_UP (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTC|GPIO_PIN15) +# define SENSE_PH1 0b10 /* Floating pulled as set */ +# define SENSE_PH2 0b01 /* Driven as tied */ + +#define GPIO_USART1_RX_SPEKTRUM (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN10) + +/* Safety switch button *******************************************************/ + +/* CONNECTED TO TP8 - pulled down via SW */ + +#define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INPULLDWN|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5) + +/* Power switch controls ******************************************************/ + +#define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) +#define SPEKTRUM_POWER(_on_true) px4_arch_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_on_true)) + +#define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(GPIO_USART1_RX_SPEKTRUM, (_one_true)) +#define SPEKTRUM_RX_AS_UART() px4_arch_configgpio(GPIO_USART1_RX) +#define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_USART1_RX_SPEKTRUM) + +#define GPIO_SERVO_FAULT_DETECT 0 // (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN15) + +/* Analog inputs **************************************************************/ + +/* PWM pins **************************************************************/ + +#define GPIO_PWM1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0) +#define GPIO_PWM2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1) +#define GPIO_PWM3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8) +#define GPIO_PWM4 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9) +#define GPIO_PWM5 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN6) +#define GPIO_PWM6 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7) +#define GPIO_PWM7 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0) +#define GPIO_PWM8 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1) + +#define DIRECT_PWM_OUTPUT_CHANNELS 8 +#define BOARD_HAS_NO_CAPTURE + +/* SBUS pins *************************************************************/ + +/* XXX these should be UART pins */ +#define GPIO_SBUS_INPUT (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN11) +#define GPIO_SBUS_OUTPUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) +#define GPIO_SBUS_OENABLE 0 // (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN4) + +/* + * High-resolution timer + */ +#define HRT_TIMER 1 /* use timer1 for the HRT */ +#define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 2 */ +#define HRT_PPM_CHANNEL 1 /* use capture/compare channel 1 PA8 */ +#define GPIO_PPM_IN (GPIO_ALT|GPIO_CNF_INPULLUP|GPIO_PORTA|GPIO_PIN8) + +/* LED definitions ******************************************************************/ +/* PX4 has two LEDs that we will encode as: */ + +#define LED_STARTED 0 /* LED? */ +#define LED_HEAPALLOCATE 1 /* LED? */ +#define LED_IRQSENABLED 2 /* LED? + LED? */ +#define LED_STACKCREATED 3 /* LED? */ +#define LED_INIRQ 4 /* LED? + LED? */ +#define LED_SIGNAL 5 /* LED? + LED? */ +#define LED_ASSERTION 6 /* LED? + LED? + LED? */ +#define LED_PANIC 7 /* N/C + N/C + N/C + LED? */ + +#define BOARD_NUM_IO_TIMERS 3 + +#include diff --git a/boards/modalai/voxl2-io/src/init.c b/boards/modalai/voxl2-io/src/init.c new file mode 100644 index 000000000000..1e432eb71fbb --- /dev/null +++ b/boards/modalai/voxl2-io/src/init.c @@ -0,0 +1,133 @@ +/**************************************************************************** + * + * 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 init.c + * + * PX4FMU-specific early startup code. This file implements the + * stm32_boardinitialize() function that is called during cpu startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialization. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include +#include + +#include + +#include +#include "board_config.h" + +#include + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Protected Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void stm32_boardinitialize(void) +{ + /* configure GPIOs */ + + /* Set up for sensing HW */ + stm32_configgpio(GPIO_SENSE_PC14_DN); + stm32_configgpio(GPIO_SENSE_PC15_UP); + + /* LEDS - default to off */ + stm32_configgpio(GPIO_LED_BLUE); + stm32_configgpio(GPIO_LED_AMBER); + stm32_configgpio(GPIO_LED_SAFETY); + + stm32_configgpio(GPIO_PC14); + stm32_configgpio(GPIO_PC15); + + + stm32_configgpio(GPIO_BTN_SAFETY); + + /* spektrum power enable is active high - enable it by default */ + stm32_configgpio(GPIO_SPEKTRUM_PWR_EN); + + stm32_configgpio(GPIO_SBUS_INPUT); /* xxx alternate function */ + stm32_configgpio(GPIO_SBUS_OUTPUT); + + stm32_gpiowrite(GPIO_PWM1, true); + stm32_configgpio(GPIO_PWM1); + + stm32_gpiowrite(GPIO_PWM2, true); + stm32_configgpio(GPIO_PWM2); + + stm32_gpiowrite(GPIO_PWM3, true); + stm32_configgpio(GPIO_PWM3); + + stm32_gpiowrite(GPIO_PWM4, true); + stm32_configgpio(GPIO_PWM4); + + stm32_gpiowrite(GPIO_PWM5, true); + stm32_configgpio(GPIO_PWM5); + + stm32_gpiowrite(GPIO_PWM6, true); + stm32_configgpio(GPIO_PWM6); + + stm32_gpiowrite(GPIO_PWM7, true); + stm32_configgpio(GPIO_PWM7); + + stm32_gpiowrite(GPIO_PWM8, true); + stm32_configgpio(GPIO_PWM8); +} diff --git a/boards/modalai/voxl2-io/src/timer_config.cpp b/boards/modalai/voxl2-io/src/timer_config.cpp new file mode 100644 index 000000000000..f84e69921cb0 --- /dev/null +++ b/boards/modalai/voxl2-io/src/timer_config.cpp @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * 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::Timer2), + initIOTimer(Timer::Timer3), + initIOTimer(Timer::Timer4), +}; + +constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { + initIOTimerChannel(io_timers, {Timer::Timer2, Timer::Channel1}, {GPIO::PortA, GPIO::Pin0}), + initIOTimerChannel(io_timers, {Timer::Timer2, Timer::Channel2}, {GPIO::PortA, GPIO::Pin1}), + initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel3}, {GPIO::PortB, GPIO::Pin8}), + initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel4}, {GPIO::PortB, GPIO::Pin9}), + initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel1}, {GPIO::PortA, GPIO::Pin6}), + initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel2}, {GPIO::PortA, GPIO::Pin7}), + initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel3}, {GPIO::PortB, GPIO::Pin0}), + initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel4}, {GPIO::PortB, GPIO::Pin1}), +}; + +constexpr io_timers_channel_mapping_t io_timers_channel_mapping = + initIOTimerChannelMapping(io_timers, timer_io_channels); diff --git a/boards/modalai/voxl2-slpi/default.px4board b/boards/modalai/voxl2-slpi/default.px4board index 8dcfe4c0d3b3..b6ce6133fcdf 100644 --- a/boards/modalai/voxl2-slpi/default.px4board +++ b/boards/modalai/voxl2-slpi/default.px4board @@ -1,18 +1,23 @@ CONFIG_PLATFORM_QURT=y CONFIG_BOARD_TOOLCHAIN="qurt" -CONFIG_DRIVERS_ACTUATORS_MODAL_IO=y +CONFIG_DRIVERS_ACTUATORS_VOXL_ESC=y CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP101XX=y +CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L0X=y +CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L1X=y CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y +CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8308=y CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y CONFIG_DRIVERS_QSHELL_QURT=y -CONFIG_DRIVERS_RC_CRSF_RC=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_EKF2=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_LAND_DETECTOR=y +CONFIG_MODULES_LOAD_MON=y CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y @@ -22,5 +27,8 @@ CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_MUORB_SLPI=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIMULATION_PWM_OUT_SIM=y CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_PARAM=y CONFIG_ORB_COMMUNICATOR=y +CONFIG_PARAM_REMOTE=y diff --git a/boards/modalai/voxl2-slpi/src/CMakeLists.txt b/boards/modalai/voxl2-slpi/src/CMakeLists.txt index 7d1867d1eca2..bb35ef068a33 100644 --- a/boards/modalai/voxl2-slpi/src/CMakeLists.txt +++ b/boards/modalai/voxl2-slpi/src/CMakeLists.txt @@ -44,7 +44,10 @@ add_library(drivers_board ) # Add custom drivers for SLPI -add_subdirectory(${PX4_BOARD_DIR}/src/drivers/icm42688p) add_subdirectory(${PX4_BOARD_DIR}/src/drivers/rc_controller) add_subdirectory(${PX4_BOARD_DIR}/src/drivers/mavlink_rc_in) -add_subdirectory(${PX4_BOARD_DIR}/src/drivers/spektrum_rc) +# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/spektrum_rc) +# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/ghst_rc) +add_subdirectory(${PX4_BOARD_DIR}/src/drivers/dsp_hitl) +# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/dsp_sbus) +add_subdirectory(${PX4_BOARD_DIR}/src/drivers/elrs_led) diff --git a/boards/modalai/voxl2-slpi/src/board_config.h b/boards/modalai/voxl2-slpi/src/board_config.h index 2aa91b4b36fe..2729397967ed 100644 --- a/boards/modalai/voxl2-slpi/src/board_config.h +++ b/boards/modalai/voxl2-slpi/src/board_config.h @@ -62,4 +62,21 @@ /* * Default port for the ESC */ -#define MODAL_IO_DEFAULT_PORT "2" +#define VOXL_ESC_DEFAULT_PORT "2" + +/* + * Default port for the GHST RC + */ +#define GHST_RC_DEFAULT_PORT "7" + +/* + * Default port for M0065 +*/ +#define VOXL2_IO_DEFAULT_PORT "2" + + +/* + * M0065 PWM + */ +#define DIRECT_PWM_OUTPUT_CHANNELS 4 +#define MAX_IO_TIMERS 3 diff --git a/boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/CMakeLists.txt b/boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/CMakeLists.txt new file mode 100644 index 000000000000..65026b83cf78 --- /dev/null +++ b/boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/CMakeLists.txt @@ -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. +# +############################################################################ + +message(STATUS "Mavlink include directory: ${PX4_SOURCE_DIR}/../build/modalai_voxl2_default/mavlink/common") + +px4_add_module( + MODULE drivers__modalai__dsp_hitl + MAIN dsp_hitl + INCLUDES + ${PX4_SOURCE_DIR}/src/drivers/dsp_hitl + ${PX4_SOURCE_DIR}/build/modalai_voxl2_default/mavlink/common + SRCS + dsp_hitl.cpp + DEPENDS + px4_work_queue + drivers_accelerometer + drivers_gyroscope + drivers_magnetometer + ) diff --git a/boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/dsp_hitl.cpp b/boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/dsp_hitl.cpp new file mode 100644 index 000000000000..b26f56508216 --- /dev/null +++ b/boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/dsp_hitl.cpp @@ -0,0 +1,1343 @@ +/**************************************************************************** + * + * 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 +#include + +#include +#include +#include + +#include + +#include + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include // For DeviceId union + +#include +#include + +#include + +#include + +#define ASYNC_UART_READ_WAIT_US 2000 + +extern "C" { __EXPORT int dsp_hitl_main(int argc, char *argv[]); } + +namespace dsp_hitl +{ + +using matrix::wrap_2pi; + +static bool _is_running = false; +volatile bool _task_should_exit = false; +static px4_task_t _task_handle = -1; +int _uart_fd = -1; +bool _debug = false; +std::string port = "2"; +int baudrate = 921600; +const unsigned mode_flag_custom = 1; +const unsigned mode_flag_armed = 128; +bool _send_mag = false; +bool _send_distance = false; + +uORB::Publication _battery_pub{ORB_ID(battery_status)}; +uORB::PublicationMulti _sensor_gps_pub{ORB_ID(sensor_gps)}; +uORB::Publication _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)}; +uORB::Publication _mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)}; +uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; +uORB::Publication _esc_status_pub{ORB_ID(esc_status)}; +uORB::Publication _distance_sensor_pub{ORB_ID(distance_sensor)}; +uORB::Publication _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)}; +uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; + +int32_t _output_functions[actuator_outputs_s::NUM_ACTUATOR_OUTPUTS] {}; + +// hil_sensor and hil_state_quaternion +enum SensorSource { + ACCEL = 0b111, + GYRO = 0b111000, + MAG = 0b111000000, + BARO = 0b1101000000000, + DIFF_PRESS = 0b10000000000 +}; + +PX4Accelerometer *_px4_accel{nullptr}; +PX4Gyroscope *_px4_gyro{nullptr}; +PX4Magnetometer *_px4_mag{nullptr}; + +bool got_first_sensor_msg = false; +float x_accel = 0; +float y_accel = 0; +float z_accel = 0; +float x_gyro = 0; +float y_gyro = 0; +float z_gyro = 0; +uint64_t gyro_accel_time = 0; + +// Status counters +uint32_t heartbeat_received_counter = 0; +uint32_t heartbeat_sent_counter = 0; +uint32_t imu_counter = 0; +uint32_t hil_sensor_counter = 0; +uint32_t mag_counter = 0; +uint32_t baro_counter = 0; +uint32_t actuator_sent_counter = 0; +uint32_t odometry_received_counter = 0; +uint32_t odometry_sent_counter = 0; +uint32_t gps_received_counter = 0; +uint32_t gps_sent_counter = 0; +uint32_t distance_received_counter = 0; +uint32_t distance_sent_counter = 0; +uint32_t flow_received_counter = 0; +uint32_t flow_sent_counter = 0; +uint32_t unknown_msg_received_counter = 0; + +enum class position_source {GPS, VIO, FLOW, NUM_POSITION_SOURCES}; + +struct position_source_data_s { + char label[8]; + bool send; + bool fail; + uint32_t failure_duration; + uint64_t failure_duration_start; +} position_source_data[(int) position_source::NUM_POSITION_SOURCES] = { + {"GPS", false, false, 0, 0}, + {"VIO", false, false, 0, 0}, + {"FLOW", false, false, 0, 0} +}; + +uint64_t first_sensor_msg_timestamp = 0; +uint64_t first_sensor_report_timestamp = 0; +uint64_t last_sensor_report_timestamp = 0; + +vehicle_status_s _vehicle_status{}; +vehicle_control_mode_s _control_mode{}; +actuator_outputs_s _actuator_outputs{}; +battery_status_s _battery_status{}; + +sensor_accel_fifo_s accel_fifo{}; +sensor_gyro_fifo_s gyro_fifo{}; + +int openPort(const char *dev, speed_t speed); +int closePort(); + +int readResponse(void *buf, size_t len); +int writeResponse(void *buf, size_t len); + +int start(int argc, char *argv[]); +int stop(); +void print_status(); +void clear_status_counters(); +bool isOpen() { return _uart_fd >= 0; }; + +void usage(); +void task_main(int argc, char *argv[]); + +void *send_actuator(void *); +void send_actuator_data(); + +void handle_message_hil_sensor_dsp(mavlink_message_t *msg); +void handle_message_hil_optical_flow(mavlink_message_t *msg); +void handle_message_distance_sensor(mavlink_message_t *msg); +void handle_message_hil_gps_dsp(mavlink_message_t *msg); +void handle_message_odometry_dsp(mavlink_message_t *msg); +void handle_message_command_long_dsp(mavlink_message_t *msg); + +void handle_message_dsp(mavlink_message_t *msg); +void actuator_controls_from_outputs_dsp(mavlink_hil_actuator_controls_t *msg); +void send_esc_status(mavlink_hil_actuator_controls_t hil_act_control); + +void +handle_message_dsp(mavlink_message_t *msg) +{ + switch (msg->msgid) { + case MAVLINK_MSG_ID_HIL_SENSOR: + hil_sensor_counter++; + handle_message_hil_sensor_dsp(msg); + break; + + case MAVLINK_MSG_ID_HIL_GPS: + gps_received_counter++; + + if (position_source_data[(int) position_source::GPS].send) { handle_message_hil_gps_dsp(msg); } + + break; + + case MAVLINK_MSG_ID_ODOMETRY: + odometry_received_counter++; + + if (position_source_data[(int) position_source::VIO].send) { handle_message_odometry_dsp(msg); } + + break; + + case MAVLINK_MSG_ID_HEARTBEAT: + heartbeat_received_counter++; + + if (_debug) { PX4_INFO("Heartbeat msg received"); } + + break; + + case MAVLINK_MSG_ID_HIL_OPTICAL_FLOW: + flow_received_counter++; + + if (position_source_data[(int) position_source::FLOW].send) { handle_message_hil_optical_flow(msg); } + + break; + + case MAVLINK_MSG_ID_DISTANCE_SENSOR: + distance_received_counter++; + + if (_send_distance) { handle_message_distance_sensor(msg); } + + break; + + default: + unknown_msg_received_counter++; + + if (_debug) { PX4_INFO("Unknown msg ID: %d", msg->msgid); } + + break; + } +} + +void *send_actuator(void *) +{ + send_actuator_data(); + return nullptr; +} + +void send_actuator_data() +{ + + int _actuator_outputs_sub = orb_subscribe_multi(ORB_ID(actuator_outputs_sim), 0); + int _vehicle_control_mode_sub_ = orb_subscribe(ORB_ID(vehicle_control_mode)); + int previous_timestamp = 0; + int previous_uorb_timestamp = 0; + int differential = 0; + bool first_sent = false; + + while (true) { + bool controls_updated = false; + (void) orb_check(_vehicle_control_mode_sub_, &controls_updated); + + if (controls_updated) { + orb_copy(ORB_ID(vehicle_control_mode), _vehicle_control_mode_sub_, &_control_mode); + } + + bool actuator_updated = false; + (void) orb_check(_actuator_outputs_sub, &actuator_updated); + + uint8_t newBuf[512]; + uint16_t newBufLen = 0; + + mavlink_hil_actuator_controls_t hil_act_control; + actuator_controls_from_outputs_dsp(&hil_act_control); + + mavlink_message_t message{}; + mavlink_msg_hil_actuator_controls_encode(1, 1, &message, &hil_act_control); + + if (actuator_updated) { + orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &_actuator_outputs); + + if (_actuator_outputs.timestamp > 0) { + previous_timestamp = _actuator_outputs.timestamp; + previous_uorb_timestamp = _actuator_outputs.timestamp; + + newBufLen = mavlink_msg_to_send_buffer(newBuf, &message); + int writeRetval = writeResponse(&newBuf, newBufLen); + + actuator_sent_counter++; + + if (_debug) { PX4_INFO("Succesful write of actuator back to jMAVSim: %d at %llu", writeRetval, hrt_absolute_time()); } + + first_sent = true; + + send_esc_status(hil_act_control); + } + + } else if (! actuator_updated && first_sent && differential > 4000) { + previous_timestamp = hrt_absolute_time(); + + newBufLen = mavlink_msg_to_send_buffer(newBuf, &message); + int writeRetval = writeResponse(&newBuf, newBufLen); + + actuator_sent_counter++; + + if (_debug) { PX4_INFO("Succesful write of actuator back to jMAVSim: %d at %llu", writeRetval, hrt_absolute_time()); } + + send_esc_status(hil_act_control); + } + + differential = hrt_absolute_time() - previous_timestamp; + + px4_usleep(1000); + } +} + +void task_main(int argc, char *argv[]) +{ + int ch; + int myoptind = 1; + const char *myoptarg = nullptr; + + while ((ch = px4_getopt(argc, argv, "odmghfp:b:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'd': + _debug = true; + break; + + case 'p': + port = myoptarg; + break; + + case 'b': + baudrate = atoi(myoptarg); + break; + + case 'm': + _send_mag = true; + break; + + case 'g': + position_source_data[(int) position_source::GPS].send = true; + break; + + case 'o': + position_source_data[(int) position_source::VIO].send = true; + break; + + case 'h': + _send_distance = true; + break; + + case 'f': + position_source_data[(int) position_source::FLOW].send = true; + break; + + default: + break; + } + } + + const char *charport = port.c_str(); + (void) openPort(charport, (speed_t) baudrate); + + if ((_debug) && (isOpen())) { PX4_INFO("DSP HITL serial port initialized. Baudrate: %d", baudrate); } + + if (! isOpen()) { + PX4_ERR("DSP HITL failed to open serial port"); + return; + } + + uint64_t last_heartbeat_timestamp = hrt_absolute_time(); + uint64_t last_imu_update_timestamp = last_heartbeat_timestamp; + + _px4_accel = new PX4Accelerometer(1310988); + _px4_gyro = new PX4Gyroscope(1310988); + _px4_mag = new PX4Magnetometer(197388); + + // Create a thread for sending data to the simulator. + pthread_t sender_thread; + pthread_attr_t sender_thread_attr; + pthread_attr_init(&sender_thread_attr); + pthread_attr_setstacksize(&sender_thread_attr, PX4_STACK_ADJUSTED(8000)); + pthread_create(&sender_thread, &sender_thread_attr, send_actuator, nullptr); + pthread_attr_destroy(&sender_thread_attr); + + int _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + + _is_running = true; + + while (!_task_should_exit) { + uint8_t rx_buf[1024]; + + uint64_t timestamp = hrt_absolute_time(); + + // Send out sensor messages every 10ms + if (got_first_sensor_msg) { + uint64_t delta_time = timestamp - last_imu_update_timestamp; + + if ((imu_counter) && (delta_time > 15000)) { + PX4_WARN("Sending updates at %llu, delta %llu", timestamp, delta_time); + } + + uint64_t _px4_gyro_accel_timestamp = hrt_absolute_time(); + _px4_gyro->update(_px4_gyro_accel_timestamp, x_gyro, y_gyro, z_gyro); + _px4_accel->update(_px4_gyro_accel_timestamp, x_accel, y_accel, z_accel); + last_imu_update_timestamp = timestamp; + imu_counter++; + } + + // Check for incoming messages from the simulator + int readRetval = readResponse(&rx_buf[0], sizeof(rx_buf)); + + if (readRetval) { + //Take readRetval and convert it into mavlink msg + mavlink_message_t msg; + mavlink_status_t _status{}; + + for (int i = 0; i <= readRetval; i++) { + if (mavlink_parse_char(MAVLINK_COMM_0, rx_buf[i], &msg, &_status)) { + //PX4_INFO("Value of msg id: %i", msg.msgid); + handle_message_dsp(&msg); + } + } + } + + if ((timestamp - last_heartbeat_timestamp) > 1000000) { + mavlink_heartbeat_t hb = {}; + mavlink_message_t hb_message = {}; + hb.autopilot = 12; + hb.base_mode |= (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) ? 128 : 0; + mavlink_msg_heartbeat_encode(1, 1, &hb_message, &hb); + + uint8_t hb_newBuf[MAVLINK_MAX_PACKET_LEN]; + uint16_t hb_newBufLen = 0; + hb_newBufLen = mavlink_msg_to_send_buffer(hb_newBuf, &hb_message); + (void) writeResponse(&hb_newBuf, hb_newBufLen); + last_heartbeat_timestamp = timestamp; + heartbeat_sent_counter++; + } + + bool vehicle_updated = false; + (void) orb_check(_vehicle_status_sub, &vehicle_updated); + + if (vehicle_updated) { + // PX4_INFO("Value of updated vehicle status: %d", vehicle_updated); + orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); + } + + uint64_t elapsed_time = hrt_absolute_time() - timestamp; + // if (elapsed_time < 10000) usleep(10000 - elapsed_time); + + if (elapsed_time < 5000) { usleep(5000 - elapsed_time); } + } + + _is_running = false; +} + +void send_esc_status(mavlink_hil_actuator_controls_t hil_act_control) +{ + esc_status_s esc_status{}; + esc_status.timestamp = hrt_absolute_time(); + const int max_esc_count = math::min(actuator_outputs_s::NUM_ACTUATOR_OUTPUTS, esc_status_s::CONNECTED_ESC_MAX); + + const bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + int max_esc_index = 0; + _battery_status_sub.update(&_battery_status); + + for (int i = 0; i < max_esc_count; i++) { + if (_output_functions[i] != 0) { + max_esc_index = i; + } + + esc_status.esc[i].actuator_function = _output_functions[i]; // TODO: this should be in pwm_sim... + esc_status.esc[i].timestamp = esc_status.timestamp; + esc_status.esc[i].esc_errorcount = 0; // TODO + esc_status.esc[i].esc_voltage = _battery_status.voltage_v; + esc_status.esc[i].esc_current = armed ? 1.0f + math::abs_t(hil_act_control.controls[i]) * 15.0f : + 0.0f; // TODO: magic number + esc_status.esc[i].esc_rpm = hil_act_control.controls[i] * 6000; // TODO: magic number + esc_status.esc[i].esc_temperature = 20.0 + math::abs_t((double)hil_act_control.controls[i]) * 40.0; + } + + esc_status.esc_count = max_esc_index + 1; + esc_status.esc_armed_flags = (1u << esc_status.esc_count) - 1; + esc_status.esc_online_flags = (1u << esc_status.esc_count) - 1; + + _esc_status_pub.publish(esc_status); +} + +void +handle_message_command_long_dsp(mavlink_message_t *msg) +{ + mavlink_command_long_t cmd_mavlink; + mavlink_msg_command_long_decode(msg, &cmd_mavlink); + + if (_debug) { PX4_INFO("Value of command_long.command: %d", cmd_mavlink.command); } + + mavlink_command_ack_t ack = {}; + ack.result = MAV_RESULT_UNSUPPORTED; + + mavlink_message_t ack_message = {}; + mavlink_msg_command_ack_encode(1, 1, &ack_message, &ack); + + uint8_t acknewBuf[512]; + uint16_t acknewBufLen = 0; + acknewBufLen = mavlink_msg_to_send_buffer(acknewBuf, &ack_message); + int writeRetval = writeResponse(&acknewBuf, acknewBufLen); + + if (_debug) { PX4_INFO("Succesful write of ACK back over UART: %d at %llu", writeRetval, hrt_absolute_time()); } +} + +int flow_debug_counter = 0; + +void +handle_message_hil_optical_flow(mavlink_message_t *msg) +{ + mavlink_hil_optical_flow_t flow; + mavlink_msg_hil_optical_flow_decode(msg, &flow); + + if ((_debug) && (!(flow_debug_counter % 10))) { + PX4_INFO("optflow: time: %llu, quality %d", flow.time_usec, (int) flow.quality); + PX4_INFO("optflow: x: %.2f y: %.2f", (double) flow.integrated_x, (double) flow.integrated_y); + } + + flow_debug_counter++; + + device::Device::DeviceId device_id; + device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK; + device_id.devid_s.bus = 1; + device_id.devid_s.address = msg->sysid; + device_id.devid_s.devtype = DRV_FLOW_DEVTYPE_SIM; + + sensor_optical_flow_s sensor_optical_flow{}; + + sensor_optical_flow.timestamp_sample = hrt_absolute_time(); + sensor_optical_flow.device_id = device_id.devid; + + sensor_optical_flow.pixel_flow[0] = flow.integrated_x; + sensor_optical_flow.pixel_flow[1] = flow.integrated_y; + + sensor_optical_flow.integration_timespan_us = flow.integration_time_us; + sensor_optical_flow.quality = flow.quality; + + int index = (int) position_source::FLOW; + + if (position_source_data[index].fail) { + uint32_t duration = position_source_data[index].failure_duration; + hrt_abstime start = position_source_data[index].failure_duration_start; + + if (duration) { + if (hrt_elapsed_time(&start) > (duration * 1000000)) { + PX4_INFO("Optical flow failure ending"); + position_source_data[index].fail = false; + position_source_data[index].failure_duration = 0; + position_source_data[index].failure_duration_start = 0; + + } else { + sensor_optical_flow.quality = 0; + } + + } else { + sensor_optical_flow.quality = 0; + } + } + + const matrix::Vector3f integrated_gyro(flow.integrated_xgyro, flow.integrated_ygyro, flow.integrated_zgyro); + + if (integrated_gyro.isAllFinite()) { + integrated_gyro.copyTo(sensor_optical_flow.delta_angle); + sensor_optical_flow.delta_angle_available = true; + } + + sensor_optical_flow.max_flow_rate = NAN; + sensor_optical_flow.min_ground_distance = NAN; + sensor_optical_flow.max_ground_distance = NAN; + + // Use distance value for distance sensor topic + // if (PX4_ISFINITE(flow.distance) && (flow.distance >= 0.f)) { + // // Positive value (including zero): distance known. Negative value: Unknown distance. + // sensor_optical_flow.distance_m = flow.distance; + // sensor_optical_flow.distance_available = true; + // } + + // Emulate voxl-flow-server where distance comes in a separate + // distance sensor topic message + sensor_optical_flow.distance_m = 0.0f; + sensor_optical_flow.distance_available = false; + + sensor_optical_flow.timestamp = hrt_absolute_time(); + + _sensor_optical_flow_pub.publish(sensor_optical_flow); + + flow_sent_counter++; +} + +int distance_debug_counter = 0; + +void handle_message_distance_sensor(mavlink_message_t *msg) +{ + mavlink_distance_sensor_t dist_sensor; + mavlink_msg_distance_sensor_decode(msg, &dist_sensor); + + if ((_debug) && (!(distance_debug_counter % 10))) { + PX4_INFO("distance: time: %u, quality: %u, height: %u", + dist_sensor.time_boot_ms, dist_sensor.signal_quality, + dist_sensor.current_distance); + } + + distance_debug_counter++; + + distance_sensor_s ds{}; + + device::Device::DeviceId device_id; + device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK; + device_id.devid_s.bus = 1; + device_id.devid_s.address = msg->sysid; + device_id.devid_s.devtype = DRV_DIST_DEVTYPE_MAVLINK; + + ds.timestamp = hrt_absolute_time(); /* Use system time for now, don't trust sender to attach correct timestamp */ + ds.min_distance = static_cast(dist_sensor.min_distance) * 1e-2f; /* cm to m */ + ds.max_distance = static_cast(dist_sensor.max_distance) * 1e-2f; /* cm to m */ + ds.current_distance = static_cast(dist_sensor.current_distance) * 1e-2f; /* cm to m */ + ds.variance = dist_sensor.covariance * 1e-4f; /* cm^2 to m^2 */ + ds.h_fov = dist_sensor.horizontal_fov; + ds.v_fov = dist_sensor.vertical_fov; + ds.q[0] = dist_sensor.quaternion[0]; + ds.q[1] = dist_sensor.quaternion[1]; + ds.q[2] = dist_sensor.quaternion[2]; + ds.q[3] = dist_sensor.quaternion[3]; + ds.type = dist_sensor.type; + ds.device_id = device_id.devid; + ds.orientation = dist_sensor.orientation; + + // MAVLink DISTANCE_SENSOR signal_quality value of 0 means unset/unknown + // quality value. Also it comes normalised between 1 and 100 while the uORB + // signal quality is normalised between 0 and 100. + ds.signal_quality = dist_sensor.signal_quality == 0 ? -1 : 100 * (dist_sensor.signal_quality - 1) / 99; + + _distance_sensor_pub.publish(ds); + + distance_sent_counter++; +} + +void +handle_message_odometry_dsp(mavlink_message_t *msg) +{ + mavlink_odometry_t odom_in; + mavlink_msg_odometry_decode(msg, &odom_in); + + odometry_sent_counter++; + + // fill vehicle_odometry from Mavlink ODOMETRY + vehicle_odometry_s odom{}; + uint64_t timestamp = hrt_absolute_time(); + odom.timestamp_sample = timestamp; + + const matrix::Vector3f odom_in_p(odom_in.x, odom_in.y, odom_in.z); + + // position x/y/z (m) + if (odom_in_p.isAllFinite()) { + // frame_id: Coordinate frame of reference for the pose data. + switch (odom_in.frame_id) { + case MAV_FRAME_LOCAL_NED: + // NED local tangent frame (x: North, y: East, z: Down) with origin fixed relative to earth. + odom.pose_frame = vehicle_odometry_s::POSE_FRAME_NED; + odom_in_p.copyTo(odom.position); + break; + + case MAV_FRAME_LOCAL_ENU: + // ENU local tangent frame (x: East, y: North, z: Up) with origin fixed relative to earth. + odom.pose_frame = vehicle_odometry_s::POSE_FRAME_NED; + odom.position[0] = odom_in.y; // y: North + odom.position[1] = odom_in.x; // x: East + odom.position[2] = -odom_in.z; // z: Up + break; + + case MAV_FRAME_LOCAL_FRD: + // FRD local tangent frame (x: Forward, y: Right, z: Down) with origin fixed relative to earth. + odom.pose_frame = vehicle_odometry_s::POSE_FRAME_FRD; + odom_in_p.copyTo(odom.position); + break; + + case MAV_FRAME_LOCAL_FLU: + // FLU local tangent frame (x: Forward, y: Left, z: Up) with origin fixed relative to earth. + odom.pose_frame = vehicle_odometry_s::POSE_FRAME_FRD; + odom.position[0] = odom_in.x; // x: Forward + odom.position[1] = -odom_in.y; // y: Left + odom.position[2] = -odom_in.z; // z: Up + break; + + default: + break; + } + + // pose_covariance + // Row-major representation of a 6x6 pose cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw) + // first six entries are the first ROW, next five entries are the second ROW, etc. + if (odom_in.estimator_type != MAV_ESTIMATOR_TYPE_NAIVE) { + switch (odom_in.frame_id) { + case MAV_FRAME_LOCAL_NED: + case MAV_FRAME_LOCAL_FRD: + case MAV_FRAME_LOCAL_FLU: + // position variances copied directly + odom.position_variance[0] = odom_in.pose_covariance[0]; // X row 0, col 0 + odom.position_variance[1] = odom_in.pose_covariance[6]; // Y row 1, col 1 + odom.position_variance[2] = odom_in.pose_covariance[11]; // Z row 2, col 2 + break; + + case MAV_FRAME_LOCAL_ENU: + // ENU local tangent frame (x: East, y: North, z: Up) with origin fixed relative to earth. + odom.position_variance[0] = odom_in.pose_covariance[6]; // Y row 1, col 1 + odom.position_variance[1] = odom_in.pose_covariance[0]; // X row 0, col 0 + odom.position_variance[2] = odom_in.pose_covariance[11]; // Z row 2, col 2 + break; + + default: + break; + } + } + } + + // q: the quaternion of the ODOMETRY msg represents a rotation from body frame to a local frame + if (matrix::Quatf(odom_in.q).isAllFinite()) { + + odom.q[0] = odom_in.q[0]; + odom.q[1] = odom_in.q[1]; + odom.q[2] = odom_in.q[2]; + odom.q[3] = odom_in.q[3]; + + // pose_covariance (roll, pitch, yaw) + // states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc. + // TODO: fix pose_covariance for MAV_FRAME_LOCAL_ENU, MAV_FRAME_LOCAL_FLU + if (odom_in.estimator_type != MAV_ESTIMATOR_TYPE_NAIVE) { + odom.orientation_variance[0] = odom_in.pose_covariance[15]; // R row 3, col 3 + odom.orientation_variance[1] = odom_in.pose_covariance[18]; // P row 4, col 4 + odom.orientation_variance[2] = odom_in.pose_covariance[20]; // Y row 5, col 5 + } + } + + const matrix::Vector3f odom_in_v(odom_in.vx, odom_in.vy, odom_in.vz); + + // velocity vx/vy/vz (m/s) + if (odom_in_v.isAllFinite()) { + // child_frame_id: Coordinate frame of reference for the velocity in free space (twist) data. + switch (odom_in.child_frame_id) { + case MAV_FRAME_LOCAL_NED: + // NED local tangent frame (x: North, y: East, z: Down) with origin fixed relative to earth. + odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_NED; + odom_in_v.copyTo(odom.velocity); + break; + + case MAV_FRAME_LOCAL_ENU: + // ENU local tangent frame (x: East, y: North, z: Up) with origin fixed relative to earth. + odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_NED; + odom.velocity[0] = odom_in.vy; // y: East + odom.velocity[1] = odom_in.vx; // x: North + odom.velocity[2] = -odom_in.vz; // z: Up + break; + + case MAV_FRAME_LOCAL_FRD: + // FRD local tangent frame (x: Forward, y: Right, z: Down) with origin fixed relative to earth. + odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_FRD; + odom_in_v.copyTo(odom.velocity); + break; + + case MAV_FRAME_LOCAL_FLU: + // FLU local tangent frame (x: Forward, y: Left, z: Up) with origin fixed relative to earth. + odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_FRD; + odom.velocity[0] = odom_in.vx; // x: Forward + odom.velocity[1] = -odom_in.vy; // y: Left + odom.velocity[2] = -odom_in.vz; // z: Up + break; + + case MAV_FRAME_BODY_NED: // DEPRECATED: Replaced by MAV_FRAME_BODY_FRD (2019-08). + case MAV_FRAME_BODY_OFFSET_NED: // DEPRECATED: Replaced by MAV_FRAME_BODY_FRD (2019-08). + case MAV_FRAME_BODY_FRD: + // FRD local tangent frame (x: Forward, y: Right, z: Down) with origin that travels with vehicle. + odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_BODY_FRD; + odom.velocity[0] = odom_in.vx; + odom.velocity[1] = odom_in.vy; + odom.velocity[2] = odom_in.vz; + break; + + default: + // unsupported child_frame_id + break; + } + + // velocity_covariance (vx, vy, vz) + // states: vx, vy, vz, rollspeed, pitchspeed, yawspeed; first six entries are the first ROW, next five entries are the second ROW, etc. + // TODO: fix velocity_covariance for MAV_FRAME_LOCAL_ENU, MAV_FRAME_LOCAL_FLU, MAV_FRAME_LOCAL_FLU + if (odom_in.estimator_type != MAV_ESTIMATOR_TYPE_NAIVE) { + switch (odom_in.child_frame_id) { + case MAV_FRAME_LOCAL_NED: + case MAV_FRAME_LOCAL_FRD: + case MAV_FRAME_LOCAL_FLU: + case MAV_FRAME_BODY_NED: // DEPRECATED: Replaced by MAV_FRAME_BODY_FRD (2019-08). + case MAV_FRAME_BODY_OFFSET_NED: // DEPRECATED: Replaced by MAV_FRAME_BODY_FRD (2019-08). + case MAV_FRAME_BODY_FRD: + // velocity covariances copied directly + odom.velocity_variance[0] = odom_in.velocity_covariance[0]; // X row 0, col 0 + odom.velocity_variance[1] = odom_in.velocity_covariance[6]; // Y row 1, col 1 + odom.velocity_variance[2] = odom_in.velocity_covariance[11]; // Z row 2, col 2 + break; + + case MAV_FRAME_LOCAL_ENU: + // ENU local tangent frame (x: East, y: North, z: Up) with origin fixed relative to earth. + odom.velocity_variance[0] = odom_in.velocity_covariance[6]; // Y row 1, col 1 + odom.velocity_variance[1] = odom_in.velocity_covariance[0]; // X row 0, col 0 + odom.velocity_variance[2] = odom_in.velocity_covariance[11]; // Z row 2, col 2 + break; + + default: + // unsupported child_frame_id + break; + } + } + } + + // Roll/Pitch/Yaw angular speed (rad/s) + if (PX4_ISFINITE(odom_in.rollspeed) + && PX4_ISFINITE(odom_in.pitchspeed) + && PX4_ISFINITE(odom_in.yawspeed)) { + + odom.angular_velocity[0] = odom_in.rollspeed; + odom.angular_velocity[1] = odom_in.pitchspeed; + odom.angular_velocity[2] = odom_in.yawspeed; + } + + odom.reset_counter = odom_in.reset_counter; + odom.quality = odom_in.quality; + + int index = (int) position_source::VIO; + + if (position_source_data[index].fail) { + uint32_t duration = position_source_data[index].failure_duration; + hrt_abstime start = position_source_data[index].failure_duration_start; + + if (duration) { + if (hrt_elapsed_time(&start) > (duration * 1000000)) { + PX4_INFO("VIO failure ending"); + position_source_data[index].fail = false; + position_source_data[index].failure_duration = 0; + position_source_data[index].failure_duration_start = 0; + + } else { + odom.quality = 0; + } + + } else { + odom.quality = 0; + } + } + + switch (odom_in.estimator_type) { + case MAV_ESTIMATOR_TYPE_UNKNOWN: // accept MAV_ESTIMATOR_TYPE_UNKNOWN for legacy support + case MAV_ESTIMATOR_TYPE_NAIVE: + case MAV_ESTIMATOR_TYPE_VISION: + case MAV_ESTIMATOR_TYPE_VIO: + odom.timestamp = hrt_absolute_time(); + _visual_odometry_pub.publish(odom); + break; + + case MAV_ESTIMATOR_TYPE_MOCAP: + odom.timestamp = hrt_absolute_time(); + _mocap_odometry_pub.publish(odom); + break; + + case MAV_ESTIMATOR_TYPE_GPS: + case MAV_ESTIMATOR_TYPE_GPS_INS: + case MAV_ESTIMATOR_TYPE_LIDAR: + case MAV_ESTIMATOR_TYPE_AUTOPILOT: + default: + //mavlink_log_critical(&_mavlink_log_pub, "ODOMETRY: estimator_type %" PRIu8 " unsupported\t", + // odom_in.estimator_type); + //events::send(events::ID("mavlink_rcv_odom_unsup_estimator_type"), events::Log::Error, + // "ODOMETRY: unsupported estimator_type {1}", odom_in.estimator_type); + return; + } +} + +void actuator_controls_from_outputs_dsp(mavlink_hil_actuator_controls_t *msg) +{ + memset(msg, 0, sizeof(mavlink_hil_actuator_controls_t)); + + msg->time_usec = hrt_absolute_time(); + + bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + + if (armed) { + for (unsigned i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; i++) { + msg->controls[i] = _actuator_outputs.output[i]; + } + + //PX4_INFO("Value of actuator data: %f, %f, %f, %f", (double)msg->controls[0], (double)msg->controls[1], (double)msg->controls[2], (double)msg->controls[3]); + } + + msg->mode = mode_flag_custom; + msg->mode |= (armed) ? mode_flag_armed : 0; + msg->flags = 0; +} + +int openPort(const char *dev, speed_t speed) +{ + if (_uart_fd >= 0) { + PX4_ERR("Port in use: %s (%i)", dev, errno); + return -1; + } + + _uart_fd = qurt_uart_open(dev, speed); + + if (_debug) { PX4_INFO("qurt_uart_opened"); } + + if (_uart_fd < 0) { + PX4_ERR("Error opening port: %s (%i)", dev, errno); + return -1; + } + + return 0; +} + +int closePort() +{ + _uart_fd = -1; + + return 0; +} + +int readResponse(void *buf, size_t len) +{ + if (_uart_fd < 0 || buf == NULL) { + PX4_ERR("invalid state for reading or buffer"); + return -1; + } + + return qurt_uart_read(_uart_fd, (char *) buf, len, ASYNC_UART_READ_WAIT_US); +} + +int writeResponse(void *buf, size_t len) +{ + if (_uart_fd < 0 || buf == NULL) { + PX4_ERR("invalid state for writing or buffer"); + return -1; + } + + return qurt_uart_write(_uart_fd, (const char *) buf, len); +} + +int start(int argc, char *argv[]) +{ + if (_is_running) { + PX4_WARN("already running"); + return -1; + } + + _task_should_exit = false; + + _task_handle = px4_task_spawn_cmd("dsp_hitl__main", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2000, + (px4_main_t)&task_main, + (char *const *)argv); + + if (_task_handle < 0) { + PX4_ERR("task start failed"); + return -1; + } + + return 0; +} + +int stop() +{ + if (!_is_running) { + PX4_WARN("not running"); + return -1; + } + + _task_should_exit = true; + + while (_is_running) { + usleep(200000); + PX4_INFO("."); + } + + _task_handle = -1; + return 0; +} + +void usage() +{ + PX4_INFO("Usage: dsp_hitl {start|status|clear|failure|stop}"); + PX4_INFO(" failure "); + PX4_INFO(" source: gps, vio, flow"); + PX4_INFO(" duration: 0 (toggle state), else seconds"); +} + +void print_status() +{ + PX4_INFO("Running: %s", _is_running ? "yes" : "no"); + PX4_INFO("HIL Sensor received: %i", hil_sensor_counter); + PX4_INFO("IMU updates: %i", imu_counter); + PX4_INFO("\tCurrent accel x, y, z: %f, %f, %f", double(x_accel), double(y_accel), double(z_accel)); + PX4_INFO("\tCurrent gyro x, y, z: %f, %f, %f", double(x_gyro), double(y_gyro), double(z_gyro)); + PX4_INFO("Magnetometer sent: %i", mag_counter); + PX4_INFO("Barometer sent: %i", baro_counter); + PX4_INFO("Heartbeat received: %i, sent: %i", heartbeat_received_counter, heartbeat_sent_counter); + PX4_INFO("Odometry received: %i, sent: %i", odometry_received_counter, odometry_sent_counter); + PX4_INFO("GPS received: %i, sent: %i", gps_received_counter, gps_sent_counter); + PX4_INFO("Distance sensor received: %i, sent: %i", distance_received_counter, distance_sent_counter); + PX4_INFO("Optical flow received: %i, sent: %i", flow_received_counter, flow_sent_counter); + PX4_INFO("Actuator updates sent: %i", actuator_sent_counter); + PX4_INFO("Unknown messages received: %i", unknown_msg_received_counter); +} + +void +clear_status_counters() +{ + heartbeat_received_counter = 0; + heartbeat_sent_counter = 0; + imu_counter = 0; + hil_sensor_counter = 0; + mag_counter = 0; + baro_counter = 0; + actuator_sent_counter = 0; + odometry_received_counter = 0; + odometry_sent_counter = 0; + gps_received_counter = 0; + gps_sent_counter = 0; + distance_received_counter = 0; + distance_sent_counter = 0; + flow_received_counter = 0; + flow_sent_counter = 0; + unknown_msg_received_counter = 0; +} + +void +handle_message_hil_sensor_dsp(mavlink_message_t *msg) +{ + mavlink_hil_sensor_t hil_sensor; + mavlink_msg_hil_sensor_decode(msg, &hil_sensor); + + // temperature only updated with baro + gyro_accel_time = hrt_absolute_time(); + + // temperature only updated with baro + float temperature = NAN; + + got_first_sensor_msg = true; + + if ((hil_sensor.fields_updated & SensorSource::BARO) == SensorSource::BARO) { + temperature = hil_sensor.temperature; + } + + // gyro + if ((hil_sensor.fields_updated & SensorSource::GYRO) == SensorSource::GYRO) { + if (_px4_gyro == nullptr) { + // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION + _px4_gyro = new PX4Gyroscope(1310988); + } + + if (_px4_gyro != nullptr) { + if (PX4_ISFINITE(temperature)) { + _px4_gyro->set_temperature(temperature); + } + + x_gyro = hil_sensor.xgyro; + y_gyro = hil_sensor.ygyro; + z_gyro = hil_sensor.zgyro; + } + } + + // accelerometer + if ((hil_sensor.fields_updated & SensorSource::ACCEL) == SensorSource::ACCEL) { + if (_px4_accel == nullptr) { + // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION + _px4_accel = new PX4Accelerometer(1310988); + } + + if (_px4_accel != nullptr) { + if (PX4_ISFINITE(temperature)) { + _px4_accel->set_temperature(temperature); + } + + x_accel = hil_sensor.xacc; + y_accel = hil_sensor.yacc; + z_accel = hil_sensor.zacc; + } + } + + + // magnetometer + if ((_send_mag) && ((hil_sensor.fields_updated & SensorSource::MAG) == SensorSource::MAG)) { + if (_px4_mag == nullptr) { + // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 3, ADDR: 1, TYPE: SIMULATION + _px4_mag = new PX4Magnetometer(197388); + } + + if (_px4_mag != nullptr) { + if (PX4_ISFINITE(temperature)) { + _px4_mag->set_temperature(temperature); + } + + _px4_mag->update(gyro_accel_time, hil_sensor.xmag, hil_sensor.ymag, hil_sensor.zmag); + + mag_counter++; + } + } + + // baro + if ((hil_sensor.fields_updated & SensorSource::BARO) == SensorSource::BARO) { + // publish + sensor_baro_s sensor_baro{}; + sensor_baro.timestamp_sample = gyro_accel_time; + sensor_baro.device_id = 6620172; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION + sensor_baro.pressure = hil_sensor.abs_pressure * 100.0f; // hPa to Pa + sensor_baro.temperature = hil_sensor.temperature; + sensor_baro.error_count = 0; + sensor_baro.timestamp = hrt_absolute_time(); + _sensor_baro_pub.publish(sensor_baro); + + baro_counter++; + } + + // battery status + { + battery_status_s hil_battery_status{}; + + hil_battery_status.timestamp = gyro_accel_time; + hil_battery_status.voltage_v = 16.0f; + hil_battery_status.voltage_filtered_v = 16.0f; + hil_battery_status.current_a = 10.0f; + hil_battery_status.discharged_mah = -1.0f; + hil_battery_status.connected = true; + hil_battery_status.remaining = 0.70; + hil_battery_status.time_remaining_s = NAN; + + _battery_pub.publish(hil_battery_status); + } +} + +void +handle_message_hil_gps_dsp(mavlink_message_t *msg) +{ + mavlink_hil_gps_t hil_gps; + mavlink_msg_hil_gps_decode(msg, &hil_gps); + + sensor_gps_s gps{}; + + device::Device::DeviceId device_id; + device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK; + device_id.devid_s.bus = 1; + device_id.devid_s.address = msg->sysid; + device_id.devid_s.devtype = DRV_GPS_DEVTYPE_SIM; + + gps.device_id = device_id.devid; + + 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; + + gps.satellites_used = hil_gps.satellites_visible; + gps.fix_type = hil_gps.fix_type; + + int index = (int) position_source::GPS; + + if (position_source_data[index].fail) { + uint32_t duration = position_source_data[index].failure_duration; + hrt_abstime start = position_source_data[index].failure_duration_start; + + if (duration) { + if (hrt_elapsed_time(&start) > (duration * 1000000)) { + PX4_INFO("GPS failure ending"); + position_source_data[index].fail = false; + position_source_data[index].failure_duration = 0; + position_source_data[index].failure_duration_start = 0; + + } else { + gps.satellites_used = 1; + gps.fix_type = 0; + } + + } else { + gps.satellites_used = 1; + gps.fix_type = 0; + } + } + + gps.eph = (float)hil_gps.eph * 1e-2f; // cm -> m + gps.epv = (float)hil_gps.epv * 1e-2f; // cm -> m + + gps.hdop = 0; // TODO + gps.vdop = 0; // TODO + + gps.noise_per_ms = 0; + gps.automatic_gain_control = 0; + gps.jamming_indicator = 0; + gps.jamming_state = 0; + gps.spoofing_state = 0; + + gps.vel_m_s = (float)(hil_gps.vel) / 100.0f; // cm/s -> m/s + gps.vel_n_m_s = (float)(hil_gps.vn) / 100.0f; // cm/s -> m/s + gps.vel_e_m_s = (float)(hil_gps.ve) / 100.0f; // cm/s -> m/s + gps.vel_d_m_s = (float)(hil_gps.vd) / 100.0f; // cm/s -> m/s + gps.cog_rad = ((hil_gps.cog == 65535) ? (float)NAN : matrix::wrap_2pi(math::radians( + hil_gps.cog * 1e-2f))); // cdeg -> rad + gps.vel_ned_valid = true; + + gps.timestamp_time_relative = 0; + gps.time_utc_usec = hil_gps.time_usec; + + + gps.heading = NAN; + gps.heading_offset = NAN; + + gps.timestamp = hrt_absolute_time(); + + _sensor_gps_pub.publish(gps); + + gps_sent_counter++; +} + +int +process_failure(dsp_hitl::position_source src, int duration) +{ + if (src >= position_source::NUM_POSITION_SOURCES) { + return 1; + } + + int index = (int) src; + + if (position_source_data[index].send) { + if (duration <= 0) { + // Toggle state + if (position_source_data[index].fail) { + PX4_INFO("Ending indefinite %s failure", position_source_data[index].label); + position_source_data[index].fail = false; + + } else { + PX4_INFO("Starting indefinite %s failure", position_source_data[index].label); + position_source_data[index].fail = true; + } + + position_source_data[index].failure_duration = 0; + position_source_data[index].failure_duration_start = 0; + + } else { + PX4_INFO("%s failure for %d seconds", position_source_data[index].label, duration); + position_source_data[index].fail = true; + position_source_data[index].failure_duration = duration; + position_source_data[index].failure_duration_start = hrt_absolute_time(); + } + + } else { + PX4_ERR("%s not active, cannot create failure", position_source_data[index].label); + return 1; + } + + return 0; +} + +} // End dsp_hitl namespace + +int dsp_hitl_main(int argc, char *argv[]) +{ + int myoptind = 1; + + if (argc <= 1) { + dsp_hitl::usage(); + return 1; + } + + const char *verb = argv[myoptind]; + + if (!strcmp(verb, "start")) { + return dsp_hitl::start(argc - 1, argv + 1); + + } else if (!strcmp(verb, "stop")) { + return dsp_hitl::stop(); + + } else if (!strcmp(verb, "status")) { + dsp_hitl::print_status(); + return 0; + + } else if (!strcmp(verb, "clear")) { + dsp_hitl::clear_status_counters(); + return 0; + + } else if (!strcmp(verb, "failure")) { + if (argc != 4) { + dsp_hitl::usage(); + return 1; + } + + const char *source = argv[myoptind + 1]; + int duration = atoi(argv[myoptind + 2]); + + if (!strcmp(source, "gps")) { + return dsp_hitl::process_failure(dsp_hitl::position_source::GPS, duration); + + } else if (!strcmp(source, "vio")) { + return dsp_hitl::process_failure(dsp_hitl::position_source::VIO, duration); + + } else if (!strcmp(source, "flow")) { + return dsp_hitl::process_failure(dsp_hitl::position_source::FLOW, duration); + + } else { + PX4_ERR("Unknown failure source %s, duration %d", source, duration); + dsp_hitl::usage(); + return 1; + } + + return 0; + + } else { + dsp_hitl::usage(); + return 1; + } +} diff --git a/boards/modalai/voxl2-slpi/src/drivers/dsp_sbus/CMakeLists.txt b/boards/modalai/voxl2-slpi/src/drivers/dsp_sbus/CMakeLists.txt new file mode 100644 index 000000000000..ccb76edadcac --- /dev/null +++ b/boards/modalai/voxl2-slpi/src/drivers/dsp_sbus/CMakeLists.txt @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_module( + MODULE drivers__dsp_sbus + MAIN dsp_sbus + COMPILE_FLAGS + -Wno-cast-align # TODO: fix and enable + SRCS + dsp_sbus.cpp + ) diff --git a/boards/modalai/voxl2-slpi/src/drivers/dsp_sbus/dsp_sbus.cpp b/boards/modalai/voxl2-slpi/src/drivers/dsp_sbus/dsp_sbus.cpp new file mode 100644 index 000000000000..e6b622b40365 --- /dev/null +++ b/boards/modalai/voxl2-slpi/src/drivers/dsp_sbus/dsp_sbus.cpp @@ -0,0 +1,383 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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 +#include +#include +#include +#include +#include +#include +#include +#include + +#include "protocol.h" + +#define ASYNC_UART_READ_WAIT_US 2000 + + +extern "C" { __EXPORT int dsp_sbus_main(int argc, char *argv[]); } + +namespace dsp_sbus +{ + +std::string _port = "7"; +int _uart_fd = -1; +IOPacket _packet; +bool _initialized = false; +bool _is_running = false; +uint64_t _rc_last_valid; ///< last valid timestamp +uint16_t _rc_valid_update_count = 0; + +static px4_task_t _task_handle = -1; + +uORB::PublicationMulti _rc_pub{ORB_ID(input_rc)}; + +int bus_exchange(IOPacket *packet) +{ + int ret = 0; + int read_retries = 3; + int read_succeeded = 0; + int packet_size = sizeof(IOPacket); + + (void) qurt_uart_write(_uart_fd, (const char *) packet, packet_size); + + usleep(100); + + // The UART read on SLPI is via an asynchronous service so specify a timeout + // for the return. The driver will poll periodically until the read comes in + // so this may block for a while. However, it will timeout if no read comes in. + while (read_retries) { + ret = qurt_uart_read(_uart_fd, (char *) packet, packet_size, ASYNC_UART_READ_WAIT_US); + + if (ret) { + // PX4_INFO("Read %d bytes", ret); + + /* Check CRC */ + uint8_t crc = packet->crc; + packet->crc = 0; + + if (crc != crc_packet(packet)) { + PX4_ERR("PX4IO packet CRC error"); + return -EIO; + + } else if (PKT_CODE(*packet) == PKT_CODE_CORRUPT) { + PX4_ERR("PX4IO packet corruption"); + return -EIO; + + } else { + read_succeeded = 1; + break; + } + } + + PX4_ERR("Read attempt %d failed", read_retries); + read_retries--; + } + + + if (! read_succeeded) { + return -EIO; + } + + return 0; +} + +int io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) +{ + /* range check the transfer */ + // if (num_values > ((_max_transfer) / sizeof(*values))) { + // PX4_ERR("io_reg_get: too many registers (%u, max %u)", num_values, _max_transfer / 2); + // return -1; + // } + + // int ret = _interface->read((page << 8) | offset, reinterpret_cast(values), num_values); + int ret = 0; + + _packet.count_code = num_values | PKT_CODE_READ; + _packet.page = page; + _packet.offset = offset; + + _packet.crc = 0; + _packet.crc = crc_packet(&_packet); + + ret = bus_exchange(&_packet); + + if (ret != 0) { + // PX4_ERR("px4io io_reg_get(%hhu,%hhu,%u): data error %d", page, offset, num_values, ret); + return -1; + } + + memcpy(values, &_packet.regs[0], num_values * 2); + + return OK; +} + +uint32_t io_reg_get(uint8_t page, uint8_t offset) +{ + uint16_t value; + + if (io_reg_get(page, offset, &value, 1) != OK) { + // Registers are only 16 bit so any value over 0xFFFF can signal a fault + return 0xFFFFFFFF; + } + + return value; +} + +int initialize() +{ + if (_initialized) { + // Already successfully initialized + return 0; + } + + if (_uart_fd < 0) { + _uart_fd = qurt_uart_open(_port.c_str(), 921600); + } + + if (_uart_fd < 0) { + PX4_ERR("Open failed in %s", __FUNCTION__); + return -1; + } + + // Verify connectivity and version number + unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); + + if (protocol != PX4IO_PROTOCOL_VERSION) { + PX4_ERR("dsp_sbus version error: %u", protocol); + _uart_fd = -1; + return -1; + } + + _initialized = true; + + return 0; +} + +void dsp_sbus_task() +{ + + uint16_t status_regs[2] {}; + input_rc_s rc_val; + const unsigned prolog = (PX4IO_P_RAW_RC_BASE - PX4IO_P_RAW_RC_COUNT); + uint16_t rc_regs[input_rc_s::RC_INPUT_MAX_CHANNELS + prolog]; + uint32_t channel_count = 0; + + _is_running = true; + + while (true) { + + usleep(20000); // Update every 20ms + + memset(&rc_val, 0, sizeof(input_rc_s)); + + if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, &status_regs[0], + sizeof(status_regs) / sizeof(status_regs[0])) == OK) { + // PX4_INFO("dsp_sbus status 0x%.4x", status_regs[0]); + // PX4_INFO("dsp_sbus alarms 0x%.4x", status_regs[1]); + } else { + // PX4_ERR("Failed to read status / alarm registers"); + continue; + } + + /* fetch values from IO */ + + // When starting the RC flag will not be okay if the receiver isn't + // getting a signal from the transmitter. Once it does, then this flag + // will say okay even if later the signal is lost. + if (!(status_regs[0] & PX4IO_P_STATUS_FLAGS_RC_OK)) { + // PX4_INFO("RC lost status flag set"); + rc_val.rc_lost = true; + + } else { + // PX4_INFO("RC lost status flag is not set"); + rc_val.rc_lost = false; + } + + if (status_regs[0] & PX4IO_P_STATUS_FLAGS_RC_SBUS) { + rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SBUS; + // PX4_INFO("Got valid SBUS"); + + } else { + rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_UNKNOWN; + // PX4_INFO("SBUS not valid"); + } + + rc_val.timestamp = hrt_absolute_time(); + + // No point in reading the registers if we haven't acquired a transmitter signal yet + if (! rc_val.rc_lost) { + if (io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, &rc_regs[0], + sizeof(rc_regs) / sizeof(rc_regs[0])) != OK) { + // PX4_ERR("Failed to read RC registers"); + continue; + // } else { + // PX4_INFO("Successfully read RC registers"); + // PX4_INFO("Prolog: %u 0x%.4x 0x%.4x 0x%.4x 0x%.4x 0x%.4x", + // rc_regs[0], rc_regs[1], rc_regs[2], rc_regs[3], rc_regs[4], rc_regs[5]); + } + + channel_count = rc_regs[PX4IO_P_RAW_RC_COUNT]; + + // const uint16_t rc_valid_update_count = rc_regs[PX4IO_P_RAW_FRAME_COUNT]; + // const bool rc_updated = (rc_valid_update_count != _rc_valid_update_count); + // + // if (!rc_updated) { + // PX4_INFO("Didn't get an RC update indication. %u %u", rc_valid_update_count, _rc_valid_update_count); + // continue; + // } + // + // _rc_valid_update_count = rc_valid_update_count; + // + // PX4_INFO("Got an RC update indication"); + + /* limit the channel count */ + if (channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) { + // PX4_INFO("Got %u for channel count. Limiting to 18", channel_count); + channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS; + } + + rc_val.channel_count = channel_count; + // PX4_INFO("RC channel count: %u", rc_val.channel_count); + + // rc_val.rc_ppm_frame_length = rc_regs[PX4IO_P_RAW_RC_DATA]; + rc_val.rc_ppm_frame_length = 0; + + rc_val.rc_failsafe = (rc_regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + // rc_val.rc_lost = !(rc_regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_RC_OK); + rc_val.rc_lost = rc_val.rc_failsafe; + rc_val.rc_lost_frame_count = rc_regs[PX4IO_P_RAW_LOST_FRAME_COUNT]; + rc_val.rc_total_frame_count = rc_regs[PX4IO_P_RAW_FRAME_COUNT]; + + if (!rc_val.rc_lost && !rc_val.rc_failsafe) { + _rc_last_valid = rc_val.timestamp; + rc_val.rssi = rc_regs[PX4IO_P_RAW_RC_NRSSI]; + rc_val.link_quality = rc_regs[PX4IO_P_RAW_RC_NRSSI]; + + /* last thing set are the actual channel values as 16 bit values */ + for (unsigned i = 0; i < channel_count; i++) { + rc_val.values[i] = rc_regs[prolog + i]; + // PX4_INFO("RC channel %u: %.4u", i, rc_val.values[i]); + } + + /* zero the remaining fields */ + for (unsigned i = channel_count; i < (sizeof(rc_val.values) / sizeof(rc_val.values[0])); i++) { + rc_val.values[i] = 0; + } + } + + rc_val.timestamp_last_signal = _rc_last_valid; + } + + _rc_pub.publish(rc_val); + + } +} + +int start(int argc, char *argv[]) +{ + + int ch; + int myoptind = 1; + const char *myoptarg = nullptr; + + while ((ch = px4_getopt(argc, argv, "p:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'p': + _port = myoptarg; + PX4_INFO("Setting port to %s", _port.c_str()); + break; + + default: + break; + } + } + + if (! _initialized) { + if (initialize()) { + return -1; + } + } + + if (_is_running) { + PX4_WARN("Already started"); + return 0; + } + + _task_handle = px4_task_spawn_cmd("dsp_sbus_main", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2000, + (px4_main_t) &dsp_sbus_task, + (char *const *)argv); + + if (_task_handle < 0) { + PX4_ERR("task start failed"); + return -1; + } + + return 0; +} + +void +usage() +{ + PX4_INFO("Usage: dsp_sbus start [options]"); + PX4_INFO("Options: -p uart port number"); +} + +} // End namespance dsp_sbus + +int dsp_sbus_main(int argc, char *argv[]) +{ + int myoptind = 1; + + if (argc <= 1) { + dsp_sbus::usage(); + return -1; + } + + const char *verb = argv[myoptind]; + + if (!strcmp(verb, "start")) { + return dsp_sbus::start(argc - 1, argv + 1); + + } else { + dsp_sbus::usage(); + return -1; + } + + return 0; +} diff --git a/boards/modalai/voxl2-slpi/src/drivers/dsp_sbus/protocol.h b/boards/modalai/voxl2-slpi/src/drivers/dsp_sbus/protocol.h new file mode 100644 index 000000000000..3cfcee2d1211 --- /dev/null +++ b/boards/modalai/voxl2-slpi/src/drivers/dsp_sbus/protocol.h @@ -0,0 +1,405 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2017 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +/** + * @file protocol.h + * + * PX4IO interface protocol. + * + * @author Lorenz Meier + * + * Communication is performed via writes to and reads from 16-bit virtual + * registers organised into pages of 255 registers each. + * + * The first two bytes of each write select a page and offset address + * respectively. Subsequent reads and writes increment the offset within + * the page. + * + * Some pages are read- or write-only. + * + * Note that some pages may permit offset values greater than 255, which + * can only be achieved by long writes. The offset does not wrap. + * + * Writes to unimplemented registers are ignored. Reads from unimplemented + * registers return undefined values. + * + * As convention, values that would be floating point in other parts of + * the PX4 system are expressed as signed integer values scaled by 10000, + * e.g. control values range from -10000..10000. Use the REG_TO_SIGNED and + * SIGNED_TO_REG macros to convert between register representation and + * the signed version, and REG_TO_FLOAT/FLOAT_TO_REG to convert to float. + * + * Note that the implementation of readable pages prefers registers within + * readable pages to be densely packed. Page numbers do not need to be + * packed. + * + * Definitions marked [1] are only valid on PX4IOv1 boards. Likewise, + * [2] denotes definitions specific to the PX4IOv2 board. + */ + +/* Per C, this is safe for all 2's complement systems */ +#define REG_TO_SIGNED(_reg) ((int16_t)(_reg)) +#define SIGNED_TO_REG(_signed) ((uint16_t)(_signed)) + +#define REG_TO_FLOAT(_reg) ((float)REG_TO_SIGNED(_reg) / 10000.0f) +#define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)floorf((_float + 0.00005f) * 10000.0f)) + +#define REG_TO_BOOL(_reg) ((bool)(_reg)) + +#define PX4IO_PROTOCOL_VERSION 4 + +/* maximum allowable sizes on this protocol version */ +#define PX4IO_PROTOCOL_MAX_CONTROL_COUNT 8 /**< The protocol does not support more than set here, individual units might support less - see PX4IO_P_CONFIG_CONTROL_COUNT */ + +/* static configuration page */ +#define PX4IO_PAGE_CONFIG 0 +#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* PX4IO_PROTOCOL_VERSION */ +#define PX4IO_P_CONFIG_HARDWARE_VERSION 1 /* magic numbers TBD */ +#define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */ +#define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum I2C transfer size */ +#define PX4IO_P_CONFIG_CONTROL_COUNT 4 /* hardcoded max control count supported */ +#define PX4IO_P_CONFIG_ACTUATOR_COUNT 5 /* hardcoded max actuator output count */ +#define PX4IO_P_CONFIG_RC_INPUT_COUNT 6 /* hardcoded max R/C input count supported */ +#define PX4IO_P_CONFIG_ADC_INPUT_COUNT 7 /* hardcoded max ADC inputs */ +#define PX4IO_P_CONFIG_RELAY_COUNT 8 /* hardcoded # of relay outputs */ +#define PX4IO_MAX_TRANSFER_LEN 64 + +/* dynamic status page */ +#define PX4IO_PAGE_STATUS 1 +#define PX4IO_P_STATUS_FREEMEM 0 +#define PX4IO_P_STATUS_CPULOAD 1 + +#define PX4IO_P_STATUS_FLAGS 2 /* monitoring flags */ +#define PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED (1 << 0) /* arm-ok and locally armed */ +#define PX4IO_P_STATUS_FLAGS_OVERRIDE (1 << 1) /* in manual override */ +#define PX4IO_P_STATUS_FLAGS_RC_OK (1 << 2) /* RC input is valid */ +#define PX4IO_P_STATUS_FLAGS_RC_PPM (1 << 3) /* PPM input is valid */ +#define PX4IO_P_STATUS_FLAGS_RC_DSM (1 << 4) /* DSM input is valid */ +#define PX4IO_P_STATUS_FLAGS_RC_SBUS (1 << 5) /* SBUS input is valid */ +#define PX4IO_P_STATUS_FLAGS_FMU_OK (1 << 6) /* controls from FMU are valid */ +#define PX4IO_P_STATUS_FLAGS_RAW_PWM (1 << 7) /* raw PWM from FMU is bypassing the mixer */ +#define PX4IO_P_STATUS_FLAGS_MIXER_OK (1 << 8) /* mixer is OK */ +#define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 9) /* the arming state between IO and FMU is in sync */ +#define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */ +#define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */ +#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 12) /* safety is off */ +#define PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED (1 << 13) /* FMU was initialized and OK once */ +#define PX4IO_P_STATUS_FLAGS_RC_ST24 (1 << 14) /* ST24 input is valid */ +#define PX4IO_P_STATUS_FLAGS_RC_SUMD (1 << 15) /* SUMD input is valid */ + +#define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */ +#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* [1] VBatt is very close to regulator dropout */ +#define PX4IO_P_STATUS_ALARMS_TEMPERATURE (1 << 1) /* board temperature is high */ +#define PX4IO_P_STATUS_ALARMS_SERVO_CURRENT (1 << 2) /* [1] servo current limit was exceeded */ +#define PX4IO_P_STATUS_ALARMS_ACC_CURRENT (1 << 3) /* [1] accessory current limit was exceeded */ +#define PX4IO_P_STATUS_ALARMS_FMU_LOST (1 << 4) /* timed out waiting for controls from FMU */ +#define PX4IO_P_STATUS_ALARMS_RC_LOST (1 << 5) /* timed out waiting for RC input */ +#define PX4IO_P_STATUS_ALARMS_PWM_ERROR (1 << 6) /* PWM configuration or output was bad */ +#define PX4IO_P_STATUS_ALARMS_VSERVO_FAULT (1 << 7) /* [2] VServo was out of the valid range (2.5 - 5.5 V) */ + +#define PX4IO_P_STATUS_VSERVO 6 /* [2] servo rail voltage in mV */ +#define PX4IO_P_STATUS_VRSSI 7 /* [2] RSSI voltage */ +#define PX4IO_P_STATUS_PRSSI 8 /* [2] RSSI PWM value */ + +#define PX4IO_P_STATUS_MIXER 9 /* mixer actuator limit flags */ + +/* array of post-mix actuator outputs, -10000..10000 */ +#define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* array of PWM servo output values, microseconds */ +#define PX4IO_PAGE_SERVOS 3 /* 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* array of raw RC input values, microseconds */ +#define PX4IO_PAGE_RAW_RC_INPUT 4 +#define PX4IO_P_RAW_RC_COUNT 0 /* number of valid channels */ +#define PX4IO_P_RAW_RC_FLAGS 1 /* RC detail status flags */ +#define PX4IO_P_RAW_RC_FLAGS_FRAME_DROP (1 << 0) /* single frame drop */ +#define PX4IO_P_RAW_RC_FLAGS_FAILSAFE (1 << 1) /* receiver is in failsafe mode */ +#define PX4IO_P_RAW_RC_FLAGS_RC_DSM11 (1 << 2) /* DSM decoding is 11 bit mode */ +#define PX4IO_P_RAW_RC_FLAGS_MAPPING_OK (1 << 3) /* Channel mapping is ok */ +#define PX4IO_P_RAW_RC_FLAGS_RC_OK (1 << 4) /* RC reception ok */ + +#define PX4IO_P_RAW_RC_NRSSI 2 /* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */ +#define PX4IO_P_RAW_RC_DATA 3 /* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */ +#define PX4IO_P_RAW_FRAME_COUNT 4 /* Number of total received frames (wrapping counter) */ +#define PX4IO_P_RAW_LOST_FRAME_COUNT 5 /* Number of total dropped frames (wrapping counter) */ +#define PX4IO_P_RAW_RC_BASE 6 /* CONFIG_RC_INPUT_COUNT channels from here */ + +/* array of scaled RC input values, -10000..10000 */ +#define PX4IO_PAGE_RC_INPUT 5 +#define PX4IO_P_RC_VALID 0 /* bitmask of valid controls */ +#define PX4IO_P_RC_BASE 1 /* CONFIG_RC_INPUT_COUNT controls from here */ + +/* array of raw ADC values */ +#define PX4IO_PAGE_RAW_ADC_INPUT 6 /* 0..CONFIG_ADC_INPUT_COUNT-1 */ + +/* PWM servo information */ +#define PX4IO_PAGE_PWM_INFO 7 +#define PX4IO_RATE_MAP_BASE 0 /* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */ + +/* setup page */ +#define PX4IO_PAGE_SETUP 50 +#define PX4IO_P_SETUP_FEATURES 0 +#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT (1 << 0) /**< enable S.Bus v1 output */ +#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT (1 << 1) /**< enable S.Bus v2 output */ +#define PX4IO_P_SETUP_FEATURES_PWM_RSSI (1 << 2) /**< enable PWM RSSI parsing */ +#define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 3) /**< enable ADC RSSI parsing */ + +#define PX4IO_P_SETUP_ARMING 1 /* arming controls */ +#define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */ +#define PX4IO_P_SETUP_ARMING_FMU_ARMED (1 << 1) /* FMU is already armed */ +#define PX4IO_P_SETUP_ARMING_FMU_PREARMED (1 << 2) /* FMU is already prearmed */ +#define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK (1 << 3) /* OK to switch to manual override via override RC channel */ +#define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM (1 << 4) /* use custom failsafe values, not 0 values of mixer */ +#define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 5) /* OK to try in-air restart */ +#define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE (1 << 6) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */ +#define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED (1 << 7) /* Disable the IO-internal evaluation of the RC */ +#define PX4IO_P_SETUP_ARMING_LOCKDOWN (1 << 8) /* If set, the system operates normally, but won't actuate any servos */ +#define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE (1 << 9) /* If set, the system will always output the failsafe values */ +#define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE (1 << 10) /* If set, the system will never return from a failsafe, but remain in failsafe once triggered. */ +#define PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE (1 << 11) /* If set then on FMU failure override is immediate. Othewise it waits for the mode switch to go past the override thrshold */ + +#define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ +#define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ +#define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */ +#define PX4IO_P_SETUP_RELAYS_PAD 5 + +#define PX4IO_P_SETUP_VSERVO_SCALE 6 /* hardware rev [2] servo voltage correction factor (float) */ +#define PX4IO_P_SETUP_DSM 7 /* DSM bind state */ +enum { /* DSM bind states */ + dsm_bind_power_down = 0, + dsm_bind_power_up, + dsm_bind_set_rx_out, + dsm_bind_send_pulses, + dsm_bind_reinit_uart +}; +/* 8 */ +#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ + +#define PX4IO_P_SETUP_REBOOT_BL 10 /* reboot IO into bootloader */ +#define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */ + +#define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */ +/* storage space of 12 occupied by CRC */ +#define PX4IO_P_SETUP_FORCE_SAFETY_OFF 12 /* force safety switch into + 'armed' (PWM enabled) state - this is a non-data write and + hence index 12 can safely be used. */ +#define PX4IO_P_SETUP_RC_THR_FAILSAFE_US 13 /**< the throttle failsafe pulse length in microseconds */ + +#define PX4IO_P_SETUP_FORCE_SAFETY_ON 14 /* force safety switch into 'disarmed' (PWM disabled state) */ +#define PX4IO_FORCE_SAFETY_MAGIC 22027 /* required argument for force safety (random) */ + +#define PX4IO_P_SETUP_PWM_REVERSE 15 /**< Bitmask to reverse PWM channels 1-8 */ +#define PX4IO_P_SETUP_TRIM_ROLL 16 /**< Roll trim, in actuator units */ +#define PX4IO_P_SETUP_TRIM_PITCH 17 /**< Pitch trim, in actuator units */ +#define PX4IO_P_SETUP_TRIM_YAW 18 /**< Yaw trim, in actuator units */ +#define PX4IO_P_SETUP_SCALE_ROLL 19 /**< Roll scale, in actuator units */ +#define PX4IO_P_SETUP_SCALE_PITCH 20 /**< Pitch scale, in actuator units */ +#define PX4IO_P_SETUP_SCALE_YAW 21 /**< Yaw scale, in actuator units */ + +#define PX4IO_P_SETUP_SBUS_RATE 22 /**< frame rate of SBUS1 output in Hz */ + +#define PX4IO_P_SETUP_MOTOR_SLEW_MAX 24 /**< max motor slew rate */ + +#define PX4IO_P_SETUP_THR_MDL_FAC 25 /**< factor for modelling motor control signal output to static thrust relationship */ + +#define PX4IO_P_SETUP_THERMAL 26 /**< thermal management */ + +#define PX4IO_P_SETUP_AIRMODE 27 /**< air-mode */ + +#define PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION 28 /**< flight termination; false if the circuit breaker (CBRK_FLIGHTTERM) is set */ + +#define PX4IO_THERMAL_IGNORE UINT16_MAX +#define PX4IO_THERMAL_OFF 0 +#define PX4IO_THERMAL_FULL 10000 + +/* autopilot control values, -10000..10000 */ +#define PX4IO_PAGE_CONTROLS 51 /**< actuator control groups, one after the other, 8 wide */ +#define PX4IO_P_CONTROLS_GROUP_0 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 0) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ +#define PX4IO_P_CONTROLS_GROUP_1 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 1) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ +#define PX4IO_P_CONTROLS_GROUP_2 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 2) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ +#define PX4IO_P_CONTROLS_GROUP_3 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 3) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ + +#define PX4IO_P_CONTROLS_GROUP_VALID 64 +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0 (1 << 0) /**< group 0 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1 (1 << 1) /**< group 1 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2 (1 << 2) /**< group 2 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3 (1 << 3) /**< group 3 is valid / received */ + +/* raw text load to the mixer parser - ignores offset */ +#define PX4IO_PAGE_MIXERLOAD 52 + +/* R/C channel config */ +#define PX4IO_PAGE_RC_CONFIG 53 /**< R/C input configuration */ +#define PX4IO_P_RC_CONFIG_MIN 0 /**< lowest input value */ +#define PX4IO_P_RC_CONFIG_CENTER 1 /**< center input value */ +#define PX4IO_P_RC_CONFIG_MAX 2 /**< highest input value */ +#define PX4IO_P_RC_CONFIG_DEADZONE 3 /**< band around center that is ignored */ +#define PX4IO_P_RC_CONFIG_ASSIGNMENT 4 /**< mapped input value */ +#define PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH 100 /**< magic value for mode switch */ +#define PX4IO_P_RC_CONFIG_OPTIONS 5 /**< channel options bitmask */ +#define PX4IO_P_RC_CONFIG_OPTIONS_ENABLED (1 << 0) +#define PX4IO_P_RC_CONFIG_OPTIONS_REVERSE (1 << 1) +#define PX4IO_P_RC_CONFIG_STRIDE 6 /**< spacing between channel config data */ + +/* PWM output - overrides mixer */ +#define PX4IO_PAGE_DIRECT_PWM 54 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* PWM failsafe values - zero disables the output */ +#define PX4IO_PAGE_FAILSAFE_PWM 55 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* PWM failsafe values - zero disables the output */ +#define PX4IO_PAGE_SENSORS 56 /**< Sensors connected to PX4IO */ +#define PX4IO_P_SENSORS_ALTITUDE 0 /**< Altitude of an external sensor (HoTT or S.BUS2) */ + +/* Debug and test page - not used in normal operation */ +#define PX4IO_PAGE_TEST 127 +#define PX4IO_P_TEST_LED 0 /**< set the amber LED on/off */ + +/* PWM minimum values for certain ESCs */ +#define PX4IO_PAGE_CONTROL_MIN_PWM 106 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* PWM maximum values for certain ESCs */ +#define PX4IO_PAGE_CONTROL_MAX_PWM 107 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* PWM mtrim values for central position */ +#define PX4IO_PAGE_CONTROL_TRIM_PWM 108 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* PWM disarmed values that are active, even when SAFETY_SAFE */ +#define PX4IO_PAGE_DISARMED_PWM 109 /* 0..CONFIG_ACTUATOR_COUNT-1 */ + +/** + * As-needed mixer data upload. + * + * This message adds text to the mixer text buffer; the text + * buffer is drained as the definitions are consumed. + */ +#pragma pack(push, 1) +struct px4io_mixdata { + uint16_t f2i_mixer_magic; +#define F2I_MIXER_MAGIC 0x6d74 + + uint8_t action; +#define F2I_MIXER_ACTION_RESET 0 +#define F2I_MIXER_ACTION_APPEND 1 + + char text[0]; /* actual text size may vary */ +}; +#pragma pack(pop) + +/** + * Serial protocol encapsulation. + */ + +#define PKT_MAX_REGS 32 // by agreement w/FMU + +#pragma pack(push, 1) +struct IOPacket { + uint8_t count_code; + uint8_t crc; + uint8_t page; + uint8_t offset; + uint16_t regs[PKT_MAX_REGS]; +}; +#pragma pack(pop) + +#if (PX4IO_MAX_TRANSFER_LEN > PKT_MAX_REGS * 2) +#error The max transfer length of the IO protocol must not be larger than the IO packet size +#endif + +#define PKT_CODE_READ 0x00 /* FMU->IO read transaction */ +#define PKT_CODE_WRITE 0x40 /* FMU->IO write transaction */ +#define PKT_CODE_SUCCESS 0x00 /* IO->FMU success reply */ +#define PKT_CODE_CORRUPT 0x40 /* IO->FMU bad packet reply */ +#define PKT_CODE_ERROR 0x80 /* IO->FMU register op error reply */ + +#define PKT_CODE_MASK 0xc0 +#define PKT_COUNT_MASK 0x3f + +#define PKT_COUNT(_p) ((_p).count_code & PKT_COUNT_MASK) +#define PKT_CODE(_p) ((_p).count_code & PKT_CODE_MASK) +#define PKT_SIZE(_p) ((size_t)((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p)))) + +static const uint8_t crc8_tab[256] __attribute__((unused)) = { + 0x00, 0x07, 0x0E, 0x09, 0x1C, 0x1B, 0x12, 0x15, + 0x38, 0x3F, 0x36, 0x31, 0x24, 0x23, 0x2A, 0x2D, + 0x70, 0x77, 0x7E, 0x79, 0x6C, 0x6B, 0x62, 0x65, + 0x48, 0x4F, 0x46, 0x41, 0x54, 0x53, 0x5A, 0x5D, + 0xE0, 0xE7, 0xEE, 0xE9, 0xFC, 0xFB, 0xF2, 0xF5, + 0xD8, 0xDF, 0xD6, 0xD1, 0xC4, 0xC3, 0xCA, 0xCD, + 0x90, 0x97, 0x9E, 0x99, 0x8C, 0x8B, 0x82, 0x85, + 0xA8, 0xAF, 0xA6, 0xA1, 0xB4, 0xB3, 0xBA, 0xBD, + 0xC7, 0xC0, 0xC9, 0xCE, 0xDB, 0xDC, 0xD5, 0xD2, + 0xFF, 0xF8, 0xF1, 0xF6, 0xE3, 0xE4, 0xED, 0xEA, + 0xB7, 0xB0, 0xB9, 0xBE, 0xAB, 0xAC, 0xA5, 0xA2, + 0x8F, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9D, 0x9A, + 0x27, 0x20, 0x29, 0x2E, 0x3B, 0x3C, 0x35, 0x32, + 0x1F, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0D, 0x0A, + 0x57, 0x50, 0x59, 0x5E, 0x4B, 0x4C, 0x45, 0x42, + 0x6F, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7D, 0x7A, + 0x89, 0x8E, 0x87, 0x80, 0x95, 0x92, 0x9B, 0x9C, + 0xB1, 0xB6, 0xBF, 0xB8, 0xAD, 0xAA, 0xA3, 0xA4, + 0xF9, 0xFE, 0xF7, 0xF0, 0xE5, 0xE2, 0xEB, 0xEC, + 0xC1, 0xC6, 0xCF, 0xC8, 0xDD, 0xDA, 0xD3, 0xD4, + 0x69, 0x6E, 0x67, 0x60, 0x75, 0x72, 0x7B, 0x7C, + 0x51, 0x56, 0x5F, 0x58, 0x4D, 0x4A, 0x43, 0x44, + 0x19, 0x1E, 0x17, 0x10, 0x05, 0x02, 0x0B, 0x0C, + 0x21, 0x26, 0x2F, 0x28, 0x3D, 0x3A, 0x33, 0x34, + 0x4E, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5C, 0x5B, + 0x76, 0x71, 0x78, 0x7F, 0x6A, 0x6D, 0x64, 0x63, + 0x3E, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2C, 0x2B, + 0x06, 0x01, 0x08, 0x0F, 0x1A, 0x1D, 0x14, 0x13, + 0xAE, 0xA9, 0xA0, 0xA7, 0xB2, 0xB5, 0xBC, 0xBB, + 0x96, 0x91, 0x98, 0x9F, 0x8A, 0x8D, 0x84, 0x83, + 0xDE, 0xD9, 0xD0, 0xD7, 0xC2, 0xC5, 0xCC, 0xCB, + 0xE6, 0xE1, 0xE8, 0xEF, 0xFA, 0xFD, 0xF4, 0xF3 +}; + +static uint8_t crc_packet(struct IOPacket *pkt) __attribute__((unused)); +static uint8_t +crc_packet(struct IOPacket *pkt) +{ + uint8_t *end = (uint8_t *)(&pkt->regs[PKT_COUNT(*pkt)]); + uint8_t *p = (uint8_t *)pkt; + uint8_t c = 0; + + while (p < end) { + c = crc8_tab[c ^ * (p++)]; + } + + return c; +} diff --git a/boards/modalai/voxl2-slpi/src/drivers/elrs_led/CMakeLists.txt b/boards/modalai/voxl2-slpi/src/drivers/elrs_led/CMakeLists.txt new file mode 100644 index 000000000000..a16e655a67f6 --- /dev/null +++ b/boards/modalai/voxl2-slpi/src/drivers/elrs_led/CMakeLists.txt @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2023 ModalAI, Inc. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_module( + MODULE drivers__elrs_led + MAIN elrs_led + COMPILE_FLAGS + -Wno-cast-align # TODO: fix and enable + SRCS + elrs_led.cpp + ) diff --git a/boards/modalai/voxl2-slpi/src/drivers/elrs_led/elrs_led.cpp b/boards/modalai/voxl2-slpi/src/drivers/elrs_led/elrs_led.cpp new file mode 100644 index 000000000000..a40bd8eda219 --- /dev/null +++ b/boards/modalai/voxl2-slpi/src/drivers/elrs_led/elrs_led.cpp @@ -0,0 +1,317 @@ +/**************************************************************************** + * + * Copyright (c) 2023 ModalAI, Inc. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + +#include +#include +#include +#include +#include +#include +#include +#include +#include "elrs_led.h" + + +extern "C" { __EXPORT int elrs_led_main(int argc, char *argv[]); } + +namespace elrs_led +{ + +std::string _port = "7"; +int _uart_fd = -1; +bool _initialized = false; +bool _is_running = false; +static bool _debug = false; + +static GENERIC_CRC8 crsf_crc{}; +static LEDState _state = LEDState::DEFAULT; +static ControllerInput _off = ControllerInput::DEFAULT; +static ControllerInput _on = ControllerInput::DEFAULT; +static ControllerInput _ir = ControllerInput::DEFAULT; +static ControllerInput _cmd = ControllerInput::DEFAULT; +static ControllerInput _prev_cmd = ControllerInput::DEFAULT; +static std::map ControllerInputMap{ + {ControllerInput::DLEFT, "DLEFT"}, + {ControllerInput::DRIGHT, "DRIGHT"}, + {ControllerInput::DDOWN, "DDOWN"}, + {ControllerInput::DUP, "DUP"}, + {ControllerInput::BACK, "BACK"}, + {ControllerInput::START, "START"}, + {ControllerInput::Y, "Y"}, + {ControllerInput::B, "B"}, + {ControllerInput::A, "A"}, + {ControllerInput::X, "X"}, + {ControllerInput::STICK_RIGHT, "STICK_RIGHT"}, + {ControllerInput::STICK_LEFT, "STICK_LEFT"}, + {ControllerInput::BUMPER_RIGHT, "BUMPER_RIGHT"}, + {ControllerInput::BUMPER_LEFT, "BUMPER_LEFT"}, + {ControllerInput::DEFAULT, "Unkown"} +}; +static px4_task_t _task_handle = -1; +void debug_info(LEDState, uint8_t *); +void make_packet(LEDState, uint8_t *); + +int initialize() +{ + if (_initialized) { + // Already successfully initialized + return 0; + } + + if (_uart_fd < 0) { + _uart_fd = qurt_uart_open(_port.c_str(), 420000); + } + + if (_uart_fd < 0) { + PX4_ERR("Open failed in %s", __FUNCTION__); + return -1; + } + + _initialized = true; + + return 0; +} + +void elrs_led_task() +{ + + PX4_INFO("Starting task for elrs_led"); + + int ret = 0; + int manual_control_input_fd = orb_subscribe(ORB_ID(manual_control_input)); + uint8_t pwmPacket[11] = {0xEC, 0x09, 0x32, 0x70, 0x77, 0x6D, 0x07, 0x75, 0x00, 0x00, 0x00}; + + px4_pollfd_struct_t fds[1] = { { .fd = manual_control_input_fd, .events = POLLIN } }; + + struct manual_control_setpoint_s setpoint_req; + + _is_running = true; + + while (true) { + px4_poll(fds, 1, 10000); + + if (fds[0].revents & POLLIN) { + + orb_copy(ORB_ID(manual_control_input), manual_control_input_fd, &setpoint_req); + + _cmd = (ControllerInput)setpoint_req.aux1; + + // skip duplicate cmds + if (_cmd == _prev_cmd) { + continue; + } + + if (_cmd == _off) { + _prev_cmd = _cmd; + _state = LEDState::OFF; + make_packet(_state, pwmPacket); + ret = qurt_uart_write(_uart_fd, (char *) &pwmPacket[0], sizeof(pwmPacket)); + + if (_debug) { + debug_info(_state, pwmPacket); + } + + } else if (_cmd == _on) { + _prev_cmd = _cmd; + _state = LEDState::ON; + make_packet(_state, pwmPacket); + ret = qurt_uart_write(_uart_fd, (char *) &pwmPacket[0], sizeof(pwmPacket)); + + if (_debug) { + debug_info(_state, pwmPacket); + } + + } else if (_cmd == _ir) { + _prev_cmd = _cmd; + _state = LEDState::IR; + make_packet(_state, pwmPacket); + ret = qurt_uart_write(_uart_fd, (char *) &pwmPacket[0], sizeof(pwmPacket)); + + if (_debug) { + debug_info(_state, pwmPacket); + } + + } + + } + } +} + +int start(int argc, char *argv[]) +{ + + int ch; + int myoptind = 1; + const char *myoptarg = nullptr; + + while ((ch = px4_getopt(argc, argv, "p:o:l:i:d", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'p': + _port = myoptarg; + break; + + case 'o': + _off = getKey(ControllerInputMap, myoptarg); + break; + + case 'l': + _on = getKey(ControllerInputMap, myoptarg); + break; + + case 'i': + _ir = getKey(ControllerInputMap, myoptarg); + break; + + case 'd': + _debug = true; + break; + + default: + break; + } + } + + if (_debug) { + PX4_INFO("ELRS LED Debug Mode Enabled"); + PX4_INFO("Port: %s", _port.c_str()); + PX4_INFO("Button Configuration:"); + PX4_INFO("\tOn: %s", ControllerInputMap.at(_on).c_str()); + PX4_INFO("\tIR: %s", ControllerInputMap.at(_ir).c_str()); + PX4_INFO("\tOff: %s", ControllerInputMap.at(_off).c_str()); + } + + if (! _initialized) { + if (initialize()) { + return -1; + } + } + + if (_is_running) { + PX4_WARN("Already started"); + return 0; + } + + _task_handle = px4_task_spawn_cmd("elrs_led_main", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2000, + (px4_main_t) &elrs_led_task, + (char *const *)argv); + + if (_task_handle < 0) { + PX4_ERR("task start failed"); + return -1; + } + + return 0; +} + +void +usage() +{ + PX4_INFO("Usage: elrs_led start [options]"); + PX4_INFO("Options: -p uart port number"); + PX4_INFO("Options: -o LEDs off button"); + PX4_INFO("Options: -l Overt LEDs on button"); + PX4_INFO("Options: -i IR LEDs on button"); + PX4_INFO("Options: -d enable debug messages"); +} + +void debug_info(LEDState led_state, uint8_t *pwmPacket) +{ + PX4_INFO(""); + + if (led_state == LEDState::ON) { + PX4_INFO("Turning LEDs on"); + + } else if (led_state == LEDState::OFF) { + PX4_INFO("Turning LEDs off"); + + } else if (led_state == LEDState::IR) { + PX4_INFO("Turning IR LEDs on"); + + } else { + PX4_WARN("ELRS LED: LED state unknown: 0x%x", led_state); + } + + PX4_INFO("Wrote packet: [0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x]", + pwmPacket[0], pwmPacket[1], pwmPacket[2], pwmPacket[3], pwmPacket[4], pwmPacket[5], + pwmPacket[6], pwmPacket[7], pwmPacket[8], pwmPacket[9], pwmPacket[10]); +} + +void make_packet(LEDState led_state, uint8_t *pwmPacket) +{ + if (led_state == LEDState::OFF) { + pwmPacket[8] = 0x03; + pwmPacket[9] = 0x84; + pwmPacket[10] = crsf_crc.calc(&pwmPacket[CRSF_FRAME_NOT_COUNTED_BYTES], PWM_FRAME_SIZE - 1, 0); + + } else if (led_state == LEDState::ON) { + pwmPacket[8] = 0x05; + pwmPacket[9] = 0xAA; + pwmPacket[10] = crsf_crc.calc(&pwmPacket[CRSF_FRAME_NOT_COUNTED_BYTES], PWM_FRAME_SIZE - 1, 0); + + } else if (led_state == LEDState::IR) { + pwmPacket[8] = 0x07; + pwmPacket[9] = 0xFF; + pwmPacket[10] = crsf_crc.calc(&pwmPacket[CRSF_FRAME_NOT_COUNTED_BYTES], PWM_FRAME_SIZE - 1, 0); + + } else { + PX4_WARN("ELRS LED: Unknown LED state."); + } +} + +} // End namespance elrs_led + +int elrs_led_main(int argc, char *argv[]) +{ + int myoptind = 1; + + if (argc <= 1) { + elrs_led::usage(); + return -1; + } + + const char *verb = argv[myoptind]; + + if (!strcmp(verb, "start")) { + return elrs_led::start(argc - 1, argv + 1); + + } else { + elrs_led::usage(); + return -1; + } + + return 0; +} \ No newline at end of file diff --git a/boards/modalai/voxl2-slpi/src/drivers/elrs_led/elrs_led.h b/boards/modalai/voxl2-slpi/src/drivers/elrs_led/elrs_led.h new file mode 100644 index 000000000000..25de041d4b8a --- /dev/null +++ b/boards/modalai/voxl2-slpi/src/drivers/elrs_led/elrs_led.h @@ -0,0 +1,113 @@ +/**************************************************************************** + * + * Copyright (c) 2023 ModalAI, Inc. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include + +#define crclen 256 +#define CRSF_CRC_POLY 0xd5 +#define CRSF_FRAME_NOT_COUNTED_BYTES 2 +#define PWM_FRAME_SIZE 9 + + +enum class ControllerInput : uint32_t { + DLEFT = 0x2000, + DRIGHT = 0x4000, + DDOWN = 0x1000, + DUP = 0x800, + BACK = 0x10, + START = 0x40, + Y = 0x08, + B = 0x02, + A = 0x01, + X = 0x04, + STICK_RIGHT = 0x100, + STICK_LEFT = 0x80, + BUMPER_RIGHT = 0x400, + BUMPER_LEFT = 0x200, + DEFAULT = 0xFFFFFFFF +}; + +enum class LEDState : uint8_t { + OFF = 0x00, + ON = 0x01, + IR = 0x02, + DEFAULT = 0xFF +}; + + +class GENERIC_CRC8 +{ +public: + GENERIC_CRC8() {}; + uint8_t calc(const uint8_t *data, uint16_t len, uint8_t crc) + { + while (len--) { + crc = crc8tab[crc ^ *data++]; + } + + return crc; + } + +private: + uint8_t crc8tab[crclen] = {0, 213, 127, 170, 254, 43, 129, 84, 41, 252, 86, 131, 215, 2, 168, 125, 82, 135, + 45, 248, 172, 121, 211, 6, 123, 174, 4, 209, 133, 80, 250, 47, 164, 113, 219, 14, + 90, 143, 37, 240, 141, 88, 242, 39, 115, 166, 12, 217, 246, 35, 137, 92, 8, 221, + 119, 162, 223, 10, 160, 117, 33, 244, 94, 139, 157, 72, 226, 55, 99, 182, 28, 201, + 180, 97, 203, 30, 74, 159, 53, 224, 207, 26, 176, 101, 49, 228, 78, 155, 230, 51, + 153, 76, 24, 205, 103, 178, 57, 236, 70, 147, 199, 18, 184, 109, 16, 197, 111, 186, + 238, 59, 145, 68, 107, 190, 20, 193, 149, 64, 234, 63, 66, 151, 61, 232, 188, 105, + 195, 22, 239, 58, 144, 69, 17, 196, 110, 187, 198, 19, 185, 108, 56, 237, 71, 146, + 189, 104, 194, 23, 67, 150, 60, 233, 148, 65, 235, 62, 106, 191, 21, 192, 75, 158, + 52, 225, 181, 96, 202, 31, 98, 183, 29, 200, 156, 73, 227, 54, 25, 204, 102, 179, 231, + 50, 152, 77, 48, 229, 79, 154, 206, 27, 177, 100, 114, 167, 13, 216, 140, 89, 243, + 38, 91, 142, 36, 241, 165, 112, 218, 15, 32, 245, 95, 138, 222, 11, 161, 116, 9, 220, + 118, 163, 247, 34, 136, 93, 214, 3, 169, 124, 40, 253, 87, 130, 255, 42, 128, 85, 1, + 212, 126, 171, 132, 81, 251, 46, 122, 175, 5, 208, 173, 120, 210, 7, 83, 134, 44, 249 + }; +}; + + +ControllerInput getKey(const std::map &map, const std::string &value) +{ + for (const auto &pair : map) { + if (pair.second == value) { + return pair.first; + } + } + + return ControllerInput::DEFAULT; +} + + + diff --git a/boards/modalai/voxl2-slpi/src/drivers/icm42688p/CMakeLists.txt b/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/CMakeLists.txt similarity index 84% rename from boards/modalai/voxl2-slpi/src/drivers/icm42688p/CMakeLists.txt rename to boards/modalai/voxl2-slpi/src/drivers/ghst_rc/CMakeLists.txt index 1f8e2102a5d9..ad2576f3e6fd 100644 --- a/boards/modalai/voxl2-slpi/src/drivers/icm42688p/CMakeLists.txt +++ b/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2020 PX4 Development Team. All rights reserved. +# 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 @@ -31,18 +31,17 @@ # ############################################################################ px4_add_module( - MODULE drivers__imu__invensense__icm42688p - MAIN icm42688p + MODULE drivers__ghst_rc + MAIN ghst_rc COMPILE_FLAGS - ${MAX_CUSTOM_OPT_LEVEL} + INCLUDES + ${PX4_SOURCE_DIR}/src/drivers/rc_input + ${PX4_SOURCE_DIR}/src/lib/rc/spektrum_rssi.h SRCS - icm42688p_main.cpp - ICM42688P.cpp - ICM42688P.hpp - InvenSense_ICM42688P_registers.hpp + ghst_rc.cpp + ghst_rc.hpp + MODULE_CONFIG + module.yaml DEPENDS - px4_work_queue - drivers_accelerometer - drivers_gyroscope - drivers__device + rc ) diff --git a/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/Kconfig b/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/Kconfig new file mode 100644 index 000000000000..0d2b31437fe1 --- /dev/null +++ b/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_GHST_RC + bool "ghst_rc" + default n + ---help--- + Enable support for ghst rc diff --git a/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/ghst_rc.cpp b/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/ghst_rc.cpp new file mode 100644 index 000000000000..e213179922ad --- /dev/null +++ b/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/ghst_rc.cpp @@ -0,0 +1,312 @@ +/**************************************************************************** + * + * 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 "ghst_rc.hpp" +#include + +#include + +#include + +#include +#include + +#include +#include +#include +#include + +using namespace time_literals; + +uint32_t GhstRc::baudrate = GHST_BAUDRATE; + +GhstRc::GhstRc(const char *device) : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(GHST_RC_DEFAULT_PORT)) +{ + if (device) { + strncpy(_device, device, sizeof(_device) - 1); + _device[sizeof(_device) - 1] = '\0'; + } +} + +GhstRc::~GhstRc() +{ + perf_free(_cycle_interval_perf); + perf_free(_publish_interval_perf); +} + +int GhstRc::task_spawn(int argc, char *argv[]) +{ + bool error_flag = false; + + int myoptind = 1; + int ch; + const char *myoptarg = nullptr; + const char *device_name = nullptr; + + while ((ch = px4_getopt(argc, argv, "d:b:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'd': + device_name = myoptarg; + break; + + case 'b': + baudrate = atoi(myoptarg); + PX4_INFO("Setting GHST baudrate to %u", baudrate); + break; + + case '?': + error_flag = true; + break; + + default: + PX4_WARN("unrecognized flag"); + error_flag = true; + break; + } + } + + if (error_flag) { + return PX4_ERROR; + } + + if (!device_name) { + PX4_ERR("Valid device required"); + return PX4_ERROR; + } + + GhstRc *instance = new GhstRc(device_name); + + if (instance == nullptr) { + PX4_ERR("alloc failed"); + return PX4_ERROR; + } + + _object.store(instance); + _task_id = task_id_is_work_queue; + + instance->ScheduleNow(); + + return PX4_OK; +} + +void GhstRc::fill_rc_in(uint16_t raw_rc_count_local, + uint16_t raw_rc_values_local[GHST_MAX_NUM_CHANNELS], + hrt_abstime now, bool frame_drop, bool failsafe, + unsigned frame_drops, int rssi = -1) +{ + // fill rc_in struct for publishing + _rc_in.channel_count = raw_rc_count_local; + + if (_rc_in.channel_count > GHST_MAX_NUM_CHANNELS) { + _rc_in.channel_count = GHST_MAX_NUM_CHANNELS; + } + + unsigned valid_chans = 0; + + for (unsigned i = 0; i < _rc_in.channel_count; i++) { + _rc_in.values[i] = raw_rc_values_local[i]; + + if (raw_rc_values_local[i] != UINT16_MAX) { + valid_chans++; + } + + // once filled, reset values back to default + _raw_rc_values[i] = UINT16_MAX; + } + + _rc_in.timestamp = now; + _rc_in.timestamp_last_signal = _rc_in.timestamp; + _rc_in.rc_ppm_frame_length = 0; + + /* fake rssi if no value was provided */ + if (rssi == -1) { + _rc_in.rssi = 255; + + } else { + _rc_in.rssi = rssi; + } + + if (valid_chans == 0) { + _rc_in.rssi = 0; + } + + _rc_in.rc_failsafe = failsafe; + _rc_in.rc_lost = (valid_chans == 0); + _rc_in.rc_lost_frame_count = frame_drops; + _rc_in.rc_total_frame_count = 0; +} + +void GhstRc::Run() +{ + if (should_exit()) { + ScheduleClear(); + _rc_fd = -1; + exit_and_cleanup(); + return; + } + + if (_rc_fd < 0) { + _rc_fd = qurt_uart_open(_device, baudrate); + + if (_rc_fd < 0) { + PX4_ERR("Error opening port: %s", _device); + return; + } + + if (_rc_fd >= 0) { + _is_singlewire = true; + + // Configure serial port for GHST + ghst_config(_rc_fd); + } + + _rc_in.rssi_dbm = NAN; + _rc_in.link_quality = -1; + + } + + const hrt_abstime time_now_us = hrt_absolute_time(); + const hrt_abstime cycle_timestamp = time_now_us; + perf_count_interval(_cycle_interval_perf, time_now_us); + + // Read all available data from the serial RC input UART + int new_bytes = qurt_uart_read(_rc_fd, (char *) &_rcs_buf[0], RC_MAX_BUFFER_SIZE, 500); + + if (new_bytes > 0) { + _bytes_rx += new_bytes; + int8_t ghst_rssi = -1; + bool rc_updated = ghst_parse(cycle_timestamp, &_rcs_buf[0], new_bytes, &_raw_rc_values[0], &ghst_rssi, + &_raw_rc_count, GHST_MAX_NUM_CHANNELS); + + if (rc_updated) { + _last_packet_seen = time_now_us; + // we have a new GHST frame. Publish it. + _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_GHST; + fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0, ghst_rssi); + + // ghst telemetry works on fmu-v5 + // on other Pixhawk (-related) boards we cannot write to the RC UART + // another option is to use a different UART port +#ifdef BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT + + if (!_ghst_telemetry) { + _ghst_telemetry = new GHSTTelemetry(_rcs_fd); + } + + if (_ghst_telemetry) { + _ghst_telemetry->update(cycle_timestamp); + } + +#endif /* BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT */ + } + } + + // If no communication + if (time_now_us - _last_packet_seen > 100_ms) { + // Invalidate link statistics + _rc_in.rssi_dbm = NAN; + _rc_in.link_quality = -1; + } + + // If we have not gotten RC updates specifically + if (time_now_us - _rc_in.timestamp_last_signal > 50_ms) { + _rc_in.rc_lost = 1; + _rc_in.rc_failsafe = 1; + _rc_in.rssi_dbm = NAN; + _rc_in.link_quality = -1; + + } else { + _rc_in.rc_lost = 0; + _rc_in.rc_failsafe = 0; + } + + _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_GHST; + _rc_in.timestamp = hrt_absolute_time(); + _input_rc_pub.publish(_rc_in); + + perf_count(_publish_interval_perf); + + ScheduleDelayed(4_ms); +} + +int GhstRc::print_status() +{ + if (_device[0] != '\0') { + PX4_INFO("UART device: %s", _device); + PX4_INFO("UART RX bytes: %" PRIu32, _bytes_rx); + } + + if (_is_singlewire) { + PX4_INFO("Telemetry disabled: Singlewire RC port"); + } + + perf_print_counter(_cycle_interval_perf); + perf_print_counter(_publish_interval_perf); + + return 0; +} + +int GhstRc::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int GhstRc::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This module parses the GHST RC uplink protocol and can generate GHST downlink telemetry data + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("ghst_rc", "driver"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyS3", "", "RC device", true); + + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int ghst_rc_main(int argc, char *argv[]) +{ + return GhstRc::main(argc, argv); +} diff --git a/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/ghst_rc.hpp b/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/ghst_rc.hpp new file mode 100644 index 000000000000..004fa020fb43 --- /dev/null +++ b/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/ghst_rc.hpp @@ -0,0 +1,126 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// telemetry +#include +#include +#include +#include +#include + +#define GHST_MAX_NUM_CHANNELS (16) + +class GhstRc : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +{ +public: + GhstRc(const char *device); + ~GhstRc() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + /** @see ModuleBase::print_status() */ + int print_status() override; + + void fill_rc_in(uint16_t raw_rc_count_local, + uint16_t raw_rc_values_local[GHST_MAX_NUM_CHANNELS], + hrt_abstime now, bool frame_drop, bool failsafe, + unsigned frame_drops, int rssi); + +private: + void Run() override; + + bool SendTelemetryBattery(const uint16_t voltage, const uint16_t current, const int fuel, const uint8_t remaining); + + bool SendTelemetryGps(const int32_t latitude, const int32_t longitude, const uint16_t groundspeed, + const uint16_t gps_heading, const uint16_t altitude, const uint8_t num_satellites); + + bool SendTelemetryAttitude(const int16_t pitch, const int16_t roll, const int16_t yaw); + + bool SendTelemetryFlightMode(const char *flight_mode); + + + uORB::PublicationMulti _input_rc_pub{ORB_ID(input_rc)}; + input_rc_s _rc_in{}; + int _rc_fd{-1}; + char _device[20] {}; // device / serial port path + bool _is_singlewire{true}; + + static constexpr size_t RC_MAX_BUFFER_SIZE{64}; + uint8_t _rcs_buf[RC_MAX_BUFFER_SIZE] {}; + uint32_t _bytes_rx{0}; + + uint16_t _raw_rc_values[GHST_MAX_NUM_CHANNELS] {}; + uint16_t _raw_rc_count{}; + + hrt_abstime _last_packet_seen{0}; + + // telemetry + hrt_abstime _telemetry_update_last{0}; + static constexpr int num_data_types{4}; ///< number of different telemetry data types + int _next_type{0}; + static uint32_t baudrate; + uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + + + perf_counter_t _cycle_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")}; + perf_counter_t _publish_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": publish interval")}; + + // DEFINE_PARAMETERS( + // (ParamBool) _param_rc_ghst_tel_en + // ) +}; + diff --git a/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/module.yaml b/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/module.yaml new file mode 100644 index 000000000000..21be68b97ebe --- /dev/null +++ b/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/module.yaml @@ -0,0 +1,11 @@ +module_name: GHST RC Input Driver +serial_config: + - command: "ghst_rc start -d ${SERIAL_DEV}" + port_config_param: + name: RC_GHST_PRT_CFG + group: Serial + #default: RC + #depends_on_port: RC + description_extended: | + Ghost RC (GHST) driver. + diff --git a/boards/modalai/voxl2-slpi/src/drivers/icm42688p/ICM42688P.cpp b/boards/modalai/voxl2-slpi/src/drivers/icm42688p/ICM42688P.cpp deleted file mode 100644 index daf593d80c6f..000000000000 --- a/boards/modalai/voxl2-slpi/src/drivers/icm42688p/ICM42688P.cpp +++ /dev/null @@ -1,985 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include "ICM42688P.hpp" - -bool hitl_mode = false; - -using namespace time_literals; - -static constexpr int16_t combine(uint8_t msb, uint8_t lsb) -{ - return (msb << 8u) | lsb; -} - -ICM42688P::ICM42688P(const I2CSPIDriverConfig &config) : - // SPI(DRV_IMU_DEVTYPE_ICM42688P, MODULE_NAME, bus, device, spi_mode, bus_frequency), - // I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - // _drdy_gpio(drdy_gpio) - 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 (!hitl_mode) { - // _px4_accel = std::make_shared(get_device_id(), rotation); - // _px4_gyro = std::make_shared(get_device_id(), rotation); - ConfigureSampleRate(_px4_gyro.get_max_rate_hz()); - // _imu_server_pub.advertise(); - - } else { - ConfigureSampleRate(0); - } -} - -ICM42688P::~ICM42688P() -{ - 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); - - // if (!hitl_mode){ - // _imu_server_pub.unadvertise(); - // } -} - -int ICM42688P::init() -{ - int ret = SPI::init(); - - if (ret != PX4_OK) { - DEVICE_DEBUG("SPI::init failed (%i)", ret); - return ret; - } - - return Reset() ? 0 : -1; -} - -bool ICM42688P::Reset() -{ - _state = STATE::RESET; - DataReadyInterruptDisable(); - ScheduleClear(); - ScheduleNow(); - return true; -} - -void ICM42688P::exit_and_cleanup() -{ - DataReadyInterruptDisable(); - I2CSPIDriverBase::exit_and_cleanup(); -} - -void ICM42688P::print_status() -{ - I2CSPIDriverBase::print_status(); - - PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us); - - 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 ICM42688P::probe() -{ - for (int i = 0; i < 3; i++) { - uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I); - - if (whoami == WHOAMI) { - PX4_INFO("ICM42688P::probe successful!"); - 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::USER_BANK_0, true); - } - } - } - - return PX4_ERROR; -} - -void ICM42688P::RunImpl() -{ - PX4_INFO(">>> ICM42688P this: %p", this); - - 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(2_ms); // to be safe wait 2 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)) { - - _state = STATE::CONFIGURE; - ScheduleDelayed(10_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()) { - - // Wakeup accel and gyro after configuring registers - ScheduleDelayed(1_ms); // add a delay here to be safe - RegisterWrite(Register::BANK_0::PWR_MGMT0, PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE); - ScheduleDelayed(30_ms); // 30 ms gyro startup time, 10 ms accel from sleep to valid data - - // if configure succeeded then start reading from FIFO - _state = STATE::FIFO_READ; - - if (DataReadyInterruptConfigure()) { - _data_ready_interrupt_enabled = true; - - // backup schedule as a watchdog timeout - ScheduleDelayed(100_ms); - - } else { - PX4_ERR("ICM42688P::RunImpl interrupt configuration failed"); - - _data_ready_interrupt_enabled = false; - ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us); - } - - FIFOReset(); - - } else { - - PX4_ERR("ICM42688P::RunImpl configuration failed"); - - // 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_READ: { -#ifndef __PX4_QURT - uint32_t samples = 0; - - if (_data_ready_interrupt_enabled) { - // scheduled from interrupt if _drdy_fifo_read_samples was set as expected - if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) { - perf_count(_drdy_missed_perf); - - } else { - samples = _fifo_gyro_samples; - } - - // push backup schedule back - ScheduleDelayed(_fifo_empty_interval_us * 2); - } - - if (samples == 0) { - // check current FIFO count - const uint16_t fifo_count = FIFOReadCount(); - - if (fifo_count >= FIFO::SIZE) { - 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)); - - 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(now, 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; - } - } - - // 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(); - } - -#endif - } - - break; - } -} - -void ICM42688P::ConfigureSampleRate(int sample_rate) -{ - if (sample_rate == 0) { - sample_rate = 800; // default to 800 Hz - } - - // 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 ICM42688P::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 ICM42688P::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 ICM42688P::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 - if (!hitl_mode) { - _px4_accel.set_range(16.f * CONSTANTS_ONE_G); - _px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f); - _px4_gyro.set_range(math::radians(2000.f)); - _px4_gyro.set_scale(math::radians(1.f / 131.f)); - } - - return success; -} - -static bool interrupt_debug = false; -static uint32_t interrupt_debug_count = 0; -static const uint32_t interrupt_debug_trigger = 800; -static hrt_abstime last_interrupt_time = 0; -static hrt_abstime avg_interrupt_delta = 0; -static hrt_abstime max_interrupt_delta = 0; -static hrt_abstime min_interrupt_delta = 60 * 1000 * 1000; -static hrt_abstime cumulative_interrupt_delta = 0; - -int ICM42688P::DataReadyInterruptCallback(int irq, void *context, void *arg) -{ - hrt_abstime current_interrupt_time = hrt_absolute_time(); - - if (interrupt_debug) { - if (last_interrupt_time) { - hrt_abstime interrupt_delta_time = current_interrupt_time - last_interrupt_time; - - if (interrupt_delta_time > max_interrupt_delta) { max_interrupt_delta = interrupt_delta_time; } - - if (interrupt_delta_time < min_interrupt_delta) { min_interrupt_delta = interrupt_delta_time; } - - cumulative_interrupt_delta += interrupt_delta_time; - } - - last_interrupt_time = current_interrupt_time; - - interrupt_debug_count++; - - if (interrupt_debug_count == interrupt_debug_trigger) { - avg_interrupt_delta = cumulative_interrupt_delta / interrupt_debug_trigger; - PX4_INFO(">>> Max: %llu, Min: %llu, Avg: %llu", max_interrupt_delta, - min_interrupt_delta, avg_interrupt_delta); - interrupt_debug_count = 0; - cumulative_interrupt_delta = 0; - } - } - - static_cast(arg)->DataReady(); - - return 0; -} - -void ICM42688P::DataReady() -{ -#ifndef __PX4_QURT - uint32_t expected = 0; - - if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) { - ScheduleNow(); - } - -#else - uint16_t fifo_byte_count = FIFOReadCount(); - - FIFORead(hrt_absolute_time(), fifo_byte_count / sizeof(FIFO::DATA)); -#endif -} - -bool ICM42688P::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 ICM42688P::DataReadyInterruptDisable() -{ - if (_drdy_gpio == 0) { - return false; - } - - return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0; -} - -template -bool ICM42688P::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 ICM42688P::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 ICM42688P::RegisterWrite(T reg, uint8_t value) -{ - uint8_t cmd[2] { (uint8_t)reg, value }; - SelectRegisterBank(reg); - transfer(cmd, cmd, sizeof(cmd)); -} - -template -void ICM42688P::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 ICM42688P::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::USER_BANK_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]); -} - -// static uint32_t debug_decimator = 0; -// static hrt_abstime last_sample_time = 0; -// static bool imu_debug = true; - -bool ICM42688P::FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples) -{ - FIFOTransferBuffer buffer{}; - const size_t max_transfer_size = 10 * sizeof(FIFO::DATA) + 4; - const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 4, max_transfer_size); - SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_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 uint16_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 - uint16_t valid_samples = 0; - - // for (int i = 0; i < math::min(samples, fifo_count_samples); i++) { - for (int i = 0; i < math::min(samples, (uint16_t) 10); 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_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 (imu_debug) { - // debug_decimator++; - // if (debug_decimator == 801) { - // debug_decimator = 0; - // PX4_INFO("Initial: %u Next: %u Valid: %u Delta: %llu", samples, fifo_count_samples, valid_samples, timestamp_sample - last_sample_time); - // } - // last_sample_time = timestamp_sample; - // } - - if (valid_samples > 0) { - if (ProcessTemperature(buffer.f, valid_samples)) { - ProcessGyro(timestamp_sample, buffer.f, valid_samples); - ProcessAccel(timestamp_sample, buffer.f, valid_samples); - // Pass only most recent valid sample to IMU server - // ProcessIMU(timestamp_sample, buffer.f[valid_samples - 1]); - return true; - } - } - - return false; -} - -void ICM42688P::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_fifo_read_samples.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 ICM42688P::ProcessIMU(const hrt_abstime ×tamp_sample, const FIFO::DATA &fifo) -{ - // float accel_x = 0.0, accel_y = 0.0, accel_z = 0.0; - // float gyro_x = 0.0, gyro_y = 0.0, gyro_z = 0.0; - // - // // 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 temp_accel_x = reassemble_20bit(fifo.ACCEL_DATA_X1, fifo.ACCEL_DATA_X0, - // fifo.Ext_Accel_X_Gyro_X & 0xF0 >> 4); - // int32_t temp_accel_y = reassemble_20bit(fifo.ACCEL_DATA_Y1, fifo.ACCEL_DATA_Y0, - // fifo.Ext_Accel_Y_Gyro_Y & 0xF0 >> 4); - // int32_t temp_accel_z = reassemble_20bit(fifo.ACCEL_DATA_Z1, fifo.ACCEL_DATA_Z0, - // fifo.Ext_Accel_Z_Gyro_Z & 0xF0 >> 4); - // - // // Gyro [19:12] + Gyro [11:4] + Gyro [3:0] (bottom 4 bits of 20 bit extension byte) - // int32_t temp_gyro_x = reassemble_20bit(fifo.GYRO_DATA_X1, fifo.GYRO_DATA_X0, - // fifo.Ext_Accel_X_Gyro_X & 0x0F); - // int32_t temp_gyro_y = reassemble_20bit(fifo.GYRO_DATA_Y1, fifo.GYRO_DATA_Y0, - // fifo.Ext_Accel_Y_Gyro_Y & 0x0F); - // int32_t temp_gyro_z = reassemble_20bit(fifo.GYRO_DATA_Z1, fifo.GYRO_DATA_Z0, - // fifo.Ext_Accel_Z_Gyro_Z & 0x0F); - - // // accel samples invalid if -524288 - // if (temp_accel_x != -524288 && temp_accel_y != -524288 && temp_accel_z != -524288) { - // // shift accel by 2 (2 least significant bits are always 0) - // accel_x = (float) temp_accel_x / 4.f; - // accel_y = (float) temp_accel_y / 4.f; - // accel_z = (float) temp_accel_z / 4.f; - // - // // shift gyro by 1 (least significant bit is always 0) - // gyro_x = (float) temp_gyro_x / 2.f; - // gyro_y = (float) temp_gyro_y / 2.f; - // gyro_z = (float) temp_gyro_z / 2.f; - // - // // correct frame for publication - // // 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_y = -accel_y; - // accel_z = -accel_z; - // gyro_y = -gyro_y; - // gyro_z = -gyro_z; - // - // // Scale everything appropriately - // float accel_scale_factor = (CONSTANTS_ONE_G / 8192.f); - // accel_x *= accel_scale_factor; - // accel_y *= accel_scale_factor; - // accel_z *= accel_scale_factor; - // - // float gyro_scale_factor = math::radians(1.f / 131.f); - // gyro_x *= gyro_scale_factor; - // gyro_y *= gyro_scale_factor; - // gyro_z *= gyro_scale_factor; - // - // // Store the data in our array - // _imu_server_data.accel_x[_imu_server_index] = accel_x; - // _imu_server_data.accel_y[_imu_server_index] = accel_y; - // _imu_server_data.accel_z[_imu_server_index] = accel_z; - // _imu_server_data.gyro_x[_imu_server_index] = gyro_x; - // _imu_server_data.gyro_y[_imu_server_index] = gyro_y; - // _imu_server_data.gyro_z[_imu_server_index] = gyro_z; - // _imu_server_data.ts[_imu_server_index] = timestamp_sample; - // _imu_server_index++; - // - // // If array is full, publish the data - // if (_imu_server_index == 10) { - // _imu_server_index = 0; - // _imu_server_data.timestamp = hrt_absolute_time(); - // _imu_server_data.temperature = 0; // Not used right now - // _imu_server_pub.publish(_imu_server_data); - // } - // } -} - -void ICM42688P::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; - accel.dt = FIFO_SAMPLE_DT; - - // 18-bits of accelerometer data - bool scale_20bit = false; - - // first pass - 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 () - 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 8192 LSB/g - if (!hitl_mode) { - _px4_accel.set_scale(CONSTANTS_ONE_G / 8192.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; - } - - if (!hitl_mode) { - _px4_accel.set_scale(CONSTANTS_ONE_G / 2048.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]; - } - - if (!hitl_mode) { - _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) { - if (!hitl_mode) { - _px4_accel.updateFIFO(accel); - } - } -} - -void ICM42688P::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; - gyro.dt = FIFO_SAMPLE_DT; - - // 20-bits of gyroscope data - bool scale_20bit = false; - - // first pass - for (int i = 0; i < samples; i++) { - // 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 131 LSB/dps - if (!hitl_mode) { - _px4_gyro.set_scale(math::radians(1.f / 131.f)); - } - - } 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); - } - - if (!hitl_mode) { - _px4_gyro.set_scale(math::radians(2000.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]; - } - - if (!hitl_mode) { - _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) { - if (!hitl_mode) { - _px4_gyro.updateFIFO(gyro); - } - } -} - -bool ICM42688P::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)) { - if (!hitl_mode) { - _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/boards/modalai/voxl2-slpi/src/drivers/icm42688p/ICM42688P.hpp b/boards/modalai/voxl2-slpi/src/drivers/icm42688p/ICM42688P.hpp deleted file mode 100644 index c1aa595d70eb..000000000000 --- a/boards/modalai/voxl2-slpi/src/drivers/icm42688p/ICM42688P.hpp +++ /dev/null @@ -1,233 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file ICM42688P.hpp - * - * Driver for the Invensense ICM42688P connected via SPI. - * - */ - -#pragma once - -#include "InvenSense_ICM42688P_registers.hpp" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace InvenSense_ICM42688P; - -extern bool hitl_mode; - -class ICM42688P : public device::SPI, public I2CSPIDriver -{ -public: - // ICM42688P(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - // spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio); - ICM42688P(const I2CSPIDriverConfig &config); - ~ICM42688P() override; - - // static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - // int runtime_instance); - static void print_usage(); - - void RunImpl(); - - int init() override; - void print_status() override; - -private: - void exit_and_cleanup() override; - - // Sensor Configuration - static constexpr float IMU_ODR{8000.f}; // 8kHz accel & gyro ODR configured - static constexpr float FIFO_SAMPLE_DT{1e6f / IMU_ODR}; - static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; - static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT}; - - // maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo - // static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(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))}; - static constexpr uint32_t FIFO_MAX_SAMPLES{10}; - - // 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)), - "Invalid FIFOTransferBuffer size"); - - 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 SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force = false); - void SelectRegisterBank(Register::BANK_0 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); } - void SelectRegisterBank(Register::BANK_1 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_1); } - void SelectRegisterBank(Register::BANK_2 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_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, uint16_t samples); - void FIFOReset(); - - void ProcessIMU(const hrt_abstime ×tamp_sample, const FIFO::DATA &fifo); - 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; - - // std::shared_ptr _px4_accel; - // std::shared_ptr _px4_gyro; - 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}; - - enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0}; - - px4::atomic _drdy_fifo_read_samples{0}; - bool _data_ready_interrupt_enabled{false}; - - enum class STATE : uint8_t { - RESET, - WAIT_FOR_RESET, - CONFIGURE, - FIFO_READ, - } _state{STATE::RESET}; - - uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval - uint32_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{12}; - 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::GYRO_CONFIG0, GYRO_CONFIG0_BIT::GYRO_FS_SEL_2000_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_16G | 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::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, 0 }, - { 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_SOURCE0, INT_SOURCE0_BIT::FIFO_THS_INT1_EN, 0 }, - }; - - uint8_t _checked_register_bank1{0}; - static constexpr uint8_t size_register_bank1_cfg{4}; - 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_SET, GYRO_CONFIG_STATIC3_BIT::GYRO_AAF_DELT_CLEAR}, - { Register::BANK_1::GYRO_CONFIG_STATIC4, GYRO_CONFIG_STATIC4_BIT::GYRO_AAF_DELTSQR_LOW_SET, GYRO_CONFIG_STATIC4_BIT::GYRO_AAF_DELTSQR_LOW_CLEAR}, - { Register::BANK_1::GYRO_CONFIG_STATIC5, GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_BITSHIFT_SET | GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_DELTSQR_HIGH_SET, GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_BITSHIFT_CLEAR | GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_DELTSQR_HIGH_CLEAR}, - }; - - uint8_t _checked_register_bank2{0}; - static constexpr uint8_t size_register_bank2_cfg{3}; - 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_SET, ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DELT_CLEAR | ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DIS }, - { Register::BANK_2::ACCEL_CONFIG_STATIC3, ACCEL_CONFIG_STATIC3_BIT::ACCEL_AAF_DELTSQR_LOW_SET, ACCEL_CONFIG_STATIC3_BIT::ACCEL_AAF_DELTSQR_LOW_CLEAR }, - { Register::BANK_2::ACCEL_CONFIG_STATIC4, ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_BITSHIFT_SET | ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_DELTSQR_HIGH_SET, ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_BITSHIFT_CLEAR | ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_DELTSQR_HIGH_CLEAR }, - }; - - uint32_t _temperature_samples{0}; - - // Support for the IMU server - // uint32_t _imu_server_index{0}; - // imu_server_s _imu_server_data; - // uORB::Publication _imu_server_pub{ORB_ID(imu_server)}; - -}; diff --git a/boards/modalai/voxl2-slpi/src/drivers/icm42688p/InvenSense_ICM42688P_registers.hpp b/boards/modalai/voxl2-slpi/src/drivers/icm42688p/InvenSense_ICM42688P_registers.hpp deleted file mode 100644 index 797df2e632f8..000000000000 --- a/boards/modalai/voxl2-slpi/src/drivers/icm42688p/InvenSense_ICM42688P_registers.hpp +++ /dev/null @@ -1,439 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file InvenSense_ICM42688P_registers.hpp - * - * Invensense ICM-42688-P registers. - * - */ - -#pragma once - -#include - -namespace InvenSense_ICM42688P -{ -// 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 = 0x47; - -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, - - FIFO_CONFIG1 = 0x5F, - FIFO_CONFIG2 = 0x60, - FIFO_CONFIG3 = 0x61, - - INT_CONFIG0 = 0x63, - - 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, -}; - -}; - -//---------------- 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, -}; - -// INTF_CONFIG0 -enum INTF_CONFIG0_BIT : uint8_t { - FIFO_HOLD_LAST_DATA_EN = Bit7, - FIFO_COUNT_REC = Bit6, - FIFO_COUNT_ENDIAN = Bit5, - SENSOR_DATA_ENDIAN = Bit4, - UI_SIFS_CFG_DISABLE_I2C = Bit1 | Bit0, -}; - -// FIFO_CONFIG -enum FIFO_CONFIG_BIT : uint8_t { - // 7:6 FIFO_MODE - 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, -}; - -// 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_2000_DPS = 0, // 0b000 = ±2000dps (default) - GYRO_FS_SEL_1000_DPS = Bit5, - GYRO_FS_SEL_500_DPS = Bit6, - GYRO_FS_SEL_250_DPS = Bit6 | Bit5, - GYRO_FS_SEL_125_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_16G = 0, // 000: ±16g (default) - ACCEL_FS_SEL_8G = Bit5, - ACCEL_FS_SEL_4G = Bit6, - ACCEL_FS_SEL_2G = 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 -}; - -// FIFO_CONFIG1 -enum FIFO_CONFIG1_BIT : uint8_t { - FIFO_RESUME_PARTIAL_RD = Bit6, - FIFO_WM_GT_TH = Bit5, - FIFO_HIRES_EN = Bit4, - 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_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 { - USER_BANK_0 = 0, // 0: Select USER BANK 0. - USER_BANK_1 = Bit0, // 1: Select USER BANK 1. - USER_BANK_2 = Bit1, // 2: Select USER BANK 2. - USER_BANK_3 = Bit1 | Bit0, // 3: Select USER BANK 3. -}; - - -//---------------- BANK1 Register bits - -// GYRO_CONFIG_STATIC2 -enum GYRO_CONFIG_STATIC2_BIT : uint8_t { - GYRO_AAF_DIS = Bit1, - GYRO_NF_DIS = Bit0, -}; - -// GYRO_CONFIG_STATIC3 -enum GYRO_CONFIG_STATIC3_BIT : uint8_t { - - // 585 Hz - GYRO_AAF_DELT_SET = Bit3 | Bit2 | Bit0, //13 - GYRO_AAF_DELT_CLEAR = Bit5 | Bit4 | Bit1, - - // 213 Hz - // GYRO_AAF_DELT_SET = Bit2 | Bit0, //5 - // GYRO_AAF_DELT_CLEAR = Bit5 | Bit4 | Bit3 | Bit1, - - // 126 Hz - //GYRO_AAF_DELT_SET = Bit1 | Bit0, //3 - //GYRO_AAF_DELT_CLEAR = Bit5 | Bit4 | Bit3 | Bit2, - - // 42 Hz - // GYRO_AAF_DELT_SET = Bit0, //1 - // GYRO_AAF_DELT_CLEAR = Bit5 | Bit4 | Bit3 | Bit2 | Bit1, - -}; - -// GYRO_CONFIG_STATIC4 -enum GYRO_CONFIG_STATIC4_BIT : uint8_t { - - // 585 Hz - GYRO_AAF_DELTSQR_LOW_SET = Bit7 | Bit5 | Bit3 | Bit1, //170 - GYRO_AAF_DELTSQR_LOW_CLEAR = Bit6 | Bit4 | Bit2 | Bit0, - - // 213 Hz - // GYRO_AAF_DELTSQR_LOW_SET = Bit4 | Bit3 | Bit0, //25 - // GYRO_AAF_DELTSQR_LOW_CLEAR = Bit7 | Bit6 | Bit5 | Bit2 | Bit1, - - // 126 Hz - //GYRO_AAF_DELTSQR_LOW_SET = Bit3 | Bit0, //9 - //GYRO_AAF_DELTSQR_LOW_CLEAR = Bit7 | Bit6 | Bit5 | Bit4 | Bit2 | Bit1, - - // 42 Hz - // GYRO_AAF_DELTSQR_LOW_SET = Bit0, //1 - // GYRO_AAF_DELTSQR_LOW_CLEAR = Bit7 | Bit6 | Bit5 | Bit4 | Bit3 | Bit2 | Bit1, -}; - -// GYRO_CONFIG_STATIC5 -enum GYRO_CONFIG_STATIC5_BIT : uint8_t { - - // 585 Hz - GYRO_AAF_DELTSQR_HIGH_SET = 0, - GYRO_AAF_DELTSQR_HIGH_CLEAR = Bit3 | Bit2 | Bit1 | Bit0, - GYRO_AAF_BITSHIFT_SET = Bit7, // 8 << 4 - GYRO_AAF_BITSHIFT_CLEAR = Bit6 | Bit5 | Bit4, - - // 213 Hz - // GYRO_AAF_DELTSQR_HIGH_SET = 0, - // GYRO_AAF_DELTSQR_HIGH_CLEAR = Bit3 | Bit2 | Bit1 | Bit0, - // GYRO_AAF_BITSHIFT_SET = Bit7 | Bit5, //10 - // GYRO_AAF_BITSHIFT_CLEAR = Bit6 | Bit4, - - // 126 Hz - // GYRO_AAF_BITSHIFT_SET = Bit7 | Bit6, //12 - // GYRO_AAF_BITSHIFT_CLEAR = Bit5 | Bit4, - - // 42 Hz - // GYRO_AAF_BITSHIFT_SET = Bit7 | Bit6 | Bit5 | Bit4, //15 - // GYRO_AAF_BITSHIFT_CLEAR = 0, - - -}; - - -//---------------- BANK2 Register bits - -// ACCEL_CONFIG_STATIC2 -enum ACCEL_CONFIG_STATIC2_BIT : uint8_t { - ACCEL_AAF_DIS = Bit0, - ACCEL_AAF_DELT = Bit3 | Bit1, - - // 213 Hz - ACCEL_AAF_DELT_SET = Bit3 | Bit1, //5 - ACCEL_AAF_DELT_CLEAR = Bit6 | Bit5 | Bit4 | Bit2, - - // 42 Hz - // ACCEL_AAF_DELT_SET = Bit1, //1 - // ACCEL_AAF_DELT_CLEAR = Bit6 | Bit5 | Bit4 | Bit3 | Bit2, -}; - -// ACCEL_CONFIG_STATIC3 -enum ACCEL_CONFIG_STATIC3_BIT : uint8_t { - ACCEL_AAF_DELTSQR_LOW = Bit4 | Bit3 | Bit0, - // 213 Hz - ACCEL_AAF_DELTSQR_LOW_SET = Bit4 | Bit3 | Bit0, //25 - ACCEL_AAF_DELTSQR_LOW_CLEAR = Bit7 | Bit6 | Bit5 | Bit2 | Bit1, - - // 42 Hz - // ACCEL_AAF_DELTSQR_LOW_SET = Bit0, //1 - // ACCEL_AAF_DELTSQR_LOW_CLEAR = Bit7 | Bit6 | Bit5 | Bit4 | Bit3 | Bit2 | Bit1, - -}; - -// ACCEL_CONFIG_STATIC4 -enum ACCEL_CONFIG_STATIC4_BIT : uint8_t { - ACCEL_AAF_BITSHIFT = Bit7 | Bit5, - ACCEL_AAF_DELTSQR_HIGH = 0, - // 213 Hz - ACCEL_AAF_BITSHIFT_SET = Bit7 | Bit5, //10 - ACCEL_AAF_BITSHIFT_CLEAR = Bit6 | Bit4, - - // 42 Hz - // ACCEL_AAF_BITSHIFT_SET = Bit7 | Bit6 | Bit5 | Bit4, //15 - // ACCEL_AAF_BITSHIFT_CLEAR = 0, - - ACCEL_AAF_DELTSQR_HIGH_SET = 0, - ACCEL_AAF_DELTSQR_HIGH_CLEAR = Bit3 | Bit2 | Bit1 | 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, - 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_ICM42688P diff --git a/boards/modalai/voxl2-slpi/src/drivers/icm42688p/icm42688p_main.cpp b/boards/modalai/voxl2-slpi/src/drivers/icm42688p/icm42688p_main.cpp deleted file mode 100644 index 38deecdf57cb..000000000000 --- a/boards/modalai/voxl2-slpi/src/drivers/icm42688p/icm42688p_main.cpp +++ /dev/null @@ -1,116 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include "ICM42688P.hpp" - -#include -#include -#include - -void ICM42688P::print_usage() -{ - PRINT_MODULE_USAGE_NAME("icm42688p", "driver"); - PRINT_MODULE_USAGE_SUBCATEGORY("imu"); - PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true); - PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); - PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); -} - -// I2CSPIDriverBase *ICM42688P::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, -// int runtime_instance) -// { -// ICM42688P *instance = new ICM42688P(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, -// cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO()); -// -// if (!instance) { -// PX4_ERR("alloc failed"); -// return nullptr; -// } -// -// if (OK != instance->init()) { -// delete instance; -// return nullptr; -// } -// -// return instance; -// } - -extern "C" int icm42688p_main(int argc, char *argv[]) -{ - - for (int i = 0; i <= argc - 1; i++) { - if (std::string(argv[i]) == "-h") { - argv[i] = 0; - hitl_mode = true; - break; - } - } - - int ch; - using ThisDriver = ICM42688P; - BusCLIArguments cli{false, true}; - cli.default_spi_frequency = SPI_SPEED; - - while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { - switch (ch) { - case 'R': - cli.rotation = (enum Rotation)atoi(cli.optArg()); - break; - } - } - - const char *verb = cli.optArg(); - - if (!verb) { - ThisDriver::print_usage(); - return -1; - } - - BusInstanceIterator iterator(MODULE_NAME, cli, DRV_IMU_DEVTYPE_ICM42688P); - - 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/boards/modalai/voxl2-slpi/src/drivers/spektrum_rc/spektrum_rc.cpp b/boards/modalai/voxl2-slpi/src/drivers/spektrum_rc/spektrum_rc.cpp index c2405de1fede..0cf350edeb78 100644 --- a/boards/modalai/voxl2-slpi/src/drivers/spektrum_rc/spektrum_rc.cpp +++ b/boards/modalai/voxl2-slpi/src/drivers/spektrum_rc/spektrum_rc.cpp @@ -150,11 +150,15 @@ void task_main(int argc, char *argv[]) newbytes = read(uart_fd, &rx_buf[0], sizeof(rx_buf)); #endif + uint8_t protocol_version = rx_buf[1] & 0x0F; + if (newbytes <= 0) { if (print_msg) { PX4_INFO("Spektrum RC: Read no bytes from UART"); } - } else if (((newbytes != DSM_FRAME_SIZE) || ((rx_buf[1] & 0x0F) != 0x02)) && (! first_correct_frame_received)) { - PX4_ERR("Spektrum RC: Read something other than correct DSM frame on read. Got %d bytes. Protocol byte is 0x%.2x", + } else if (((newbytes != DSM_FRAME_SIZE) || + ((protocol_version != 0x02) && (protocol_version != 0x01))) && + (! first_correct_frame_received)) { + PX4_ERR("Spektrum RC: Invalid DSM frame. %d bytes. Protocol byte 0x%.2x", newbytes, rx_buf[1]); } else { diff --git a/boards/modalai/voxl2/cmake/init.cmake b/boards/modalai/voxl2/cmake/init.cmake new file mode 100644 index 000000000000..e5a4daad5f8b --- /dev/null +++ b/boards/modalai/voxl2/cmake/init.cmake @@ -0,0 +1,34 @@ +############################################################################ +# +# Copyright (c) 2024 ModalAI, Inc. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +include_directories(${PX4_BOARD_DIR}/libfc-sensor-api/inc) \ No newline at end of file diff --git a/boards/modalai/voxl2/cmake/link_libraries.cmake b/boards/modalai/voxl2/cmake/link_libraries.cmake index 42c9feae350f..e0b09beb7038 100644 --- a/boards/modalai/voxl2/cmake/link_libraries.cmake +++ b/boards/modalai/voxl2/cmake/link_libraries.cmake @@ -1,7 +1,7 @@ -# libfc_sensor.so is provided in the Docker build environment +# Link against the public stub version of the proprietary fc sensor library target_link_libraries(px4 PRIVATE - /home/libfc_sensor.so + ${PX4_BOARD_DIR}/libfc-sensor-api/build/libfc_sensor.so px4_layer ${module_libraries} ) diff --git a/boards/modalai/voxl2/default.px4board b/boards/modalai/voxl2/default.px4board index ed1e0ed682e5..8ac31695723f 100644 --- a/boards/modalai/voxl2/default.px4board +++ b/boards/modalai/voxl2/default.px4board @@ -2,18 +2,21 @@ CONFIG_PLATFORM_POSIX=y CONFIG_BOARD_LINUX_TARGET=y CONFIG_BOARD_TOOLCHAIN="aarch64-linux-gnu" CONFIG_BOARD_ROOTFSDIR="/data/px4" -CONFIG_DRIVERS_ACTUATORS_MODAL_IO=y +CONFIG_DRIVERS_ACTUATORS_VOXL_ESC=y +CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_OSD_MSP_OSD=y CONFIG_DRIVERS_QSHELL_POSIX=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_VOXL2_IO=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MUORB_APPS=y CONFIG_MODULES_NAVIGATOR=y -CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_SIMULATION_PWM_OUT_SIM=y +CONFIG_MODULES_UXRCE_DDS_CLIENT=y CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y CONFIG_SYSTEMCMDS_BSONDUMP=y CONFIG_SYSTEMCMDS_PARAM=y @@ -22,3 +25,4 @@ CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y CONFIG_SYSTEMCMDS_UORB=y CONFIG_SYSTEMCMDS_VER=y CONFIG_ORB_COMMUNICATOR=y +CONFIG_PARAM_PRIMARY=y diff --git a/boards/modalai/voxl2/libfc-sensor-api b/boards/modalai/voxl2/libfc-sensor-api new file mode 160000 index 000000000000..ca16e9907464 --- /dev/null +++ b/boards/modalai/voxl2/libfc-sensor-api @@ -0,0 +1 @@ +Subproject commit ca16e99074641a10d153961291243ede7720c2e2 diff --git a/boards/modalai/voxl2/scripts/build-deps.sh b/boards/modalai/voxl2/scripts/build-deps.sh new file mode 100755 index 000000000000..fe7f85486888 --- /dev/null +++ b/boards/modalai/voxl2/scripts/build-deps.sh @@ -0,0 +1,9 @@ +#!/bin/bash + +cd boards/modalai/voxl2/libfc-sensor-api +rm -fR build +mkdir build +cd build +CC=/home/4.1.0.4/tools/linaro64/bin/aarch64-linux-gnu-gcc cmake .. +make +cd ../../../../.. diff --git a/boards/modalai/voxl2/scripts/build-slpi.sh b/boards/modalai/voxl2/scripts/build-slpi.sh index 4c05f8f4f150..cbe9ef6ba925 100755 --- a/boards/modalai/voxl2/scripts/build-slpi.sh +++ b/boards/modalai/voxl2/scripts/build-slpi.sh @@ -6,6 +6,4 @@ source /home/build-env.sh make modalai_voxl2-slpi -cat build/modalai_voxl2-slpi_default/src/lib/version/build_git_version.h - echo "*** End of qurt slpi build ***" diff --git a/boards/modalai/voxl2/scripts/clean.sh b/boards/modalai/voxl2/scripts/clean.sh index 51cef9c3f242..80832b0b70f5 100755 --- a/boards/modalai/voxl2/scripts/clean.sh +++ b/boards/modalai/voxl2/scripts/clean.sh @@ -1,5 +1,8 @@ #!/bin/bash # Clean out the build artifacts -source /home/build-env.sh -make clean +# source /home/build-env.sh +# make clean + +# This gets rid of everything and starts fresh +sudo rm -fR build diff --git a/boards/modalai/voxl2/scripts/install-voxl-bins.sh b/boards/modalai/voxl2/scripts/install-voxl-bins.sh new file mode 100755 index 000000000000..d58627bcf449 --- /dev/null +++ b/boards/modalai/voxl2/scripts/install-voxl-bins.sh @@ -0,0 +1,9 @@ +#!/bin/bash + +# Push slpi image to voxl2 +adb push build/modalai_voxl2-slpi_default/platforms/qurt/libpx4.so /usr/lib/rfsa/adsp + +# Push apps processor image to voxl2 +adb push build/modalai_voxl2_default/bin/px4 /usr/bin + +adb shell sync diff --git a/boards/modalai/voxl2/scripts/install-voxl.sh b/boards/modalai/voxl2/scripts/install-voxl.sh index 37f2ede0203f..6bf21f1e564c 100755 --- a/boards/modalai/voxl2/scripts/install-voxl.sh +++ b/boards/modalai/voxl2/scripts/install-voxl.sh @@ -10,14 +10,19 @@ adb push build/modalai_voxl2_default/bin/px4 /usr/bin adb push build/modalai_voxl2_default/bin/px4-alias.sh /usr/bin adb push boards/modalai/voxl2/target/voxl-px4 /usr/bin adb push boards/modalai/voxl2/target/voxl-px4-start /usr/bin +adb push boards/modalai/voxl2/target/voxl-px4-hitl /usr/bin +adb push boards/modalai/voxl2/target/voxl-px4-hitl-start /usr/bin adb shell chmod a+x /usr/bin/px4-alias.sh adb shell chmod a+x /usr/bin/voxl-px4 adb shell chmod a+x /usr/bin/voxl-px4-start +adb shell chmod a+x /usr/bin/voxl-px4-hitl +adb shell chmod a+x /usr/bin/voxl-px4-hitl-start # Push configuration file adb shell mkdir -p /etc/modalai adb push boards/modalai/voxl2/target/voxl-px4-set-default-parameters.config /etc/modalai adb push boards/modalai/voxl2/target/voxl-px4-fake-imu-calibration.config /etc/modalai +adb push boards/modalai/voxl2/target/voxl-px4-hitl-set-default-parameters.config /etc/modalai # Make sure to setup all of the needed px4 aliases. adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-accelsim" @@ -34,6 +39,7 @@ adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-commander_tests" adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-control_allocator" adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-controllib_test" adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-dataman" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-dsp_hitl" adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-ekf2" adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-esc_calib" adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-ets_airspeed" @@ -65,7 +71,9 @@ adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-mb12xx" adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-mc_att_control" adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-mc_pos_control" adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-measairspeedsim" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-microdds_client" adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-mixer" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-voxl2_io_bridge" adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-motor_ramp" adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-modalai_gps_timer" adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-ms4525_airspeed" @@ -119,6 +127,7 @@ adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-rc_controller" adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-uart_esc_driver" adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-flight_mode_manager" adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-imu_server" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-apps_sbus" # Make sure any required directories exist adb shell "/bin/mkdir -p /data/px4/param" diff --git a/boards/modalai/voxl2/scripts/patch-gazebo.sh b/boards/modalai/voxl2/scripts/patch-gazebo.sh new file mode 100755 index 000000000000..9e8b22cf60d9 --- /dev/null +++ b/boards/modalai/voxl2/scripts/patch-gazebo.sh @@ -0,0 +1,13 @@ +#!/bin/bash + +cd Tools/simulation/gazebo-classic/sitl_gazebo-classic/src +patch < ../../../../../boards/modalai/voxl2/gazebo-docker/patch/mavlink_interface.patch +cd - + +cd Tools/simulation/gazebo-classic/sitl_gazebo-classic/models/iris_hitl +patch < ../../../../../../boards/modalai/voxl2/gazebo-docker/patch/iris_hitl.patch +cd - + +cd Tools/simulation/gazebo-classic/sitl_gazebo-classic/models/iris_vision +patch < ../../../../../../boards/modalai/voxl2/gazebo-docker/patch/iris_vision.patch +cd - \ No newline at end of file diff --git a/boards/modalai/voxl2/src/CMakeLists.txt b/boards/modalai/voxl2/src/CMakeLists.txt index f23ed05ec104..97cc043a1483 100644 --- a/boards/modalai/voxl2/src/CMakeLists.txt +++ b/boards/modalai/voxl2/src/CMakeLists.txt @@ -41,3 +41,6 @@ add_library(drivers_board board_config.h init.c ) + +# Add custom drivers +add_subdirectory(${PX4_BOARD_DIR}/src/drivers/apps_sbus) diff --git a/boards/modalai/voxl2/src/board_config.h b/boards/modalai/voxl2/src/board_config.h index 9fe0f35807a3..a83f28966bfe 100644 --- a/boards/modalai/voxl2/src/board_config.h +++ b/boards/modalai/voxl2/src/board_config.h @@ -51,4 +51,5 @@ #define BOARD_OVERRIDE_UUID "MODALAIVOXL20000" // must be of length 16 #define PX4_SOC_ARCH_ID PX4_SOC_ARCH_ID_VOXL2 -#define MODAL_IO_DEFAULT_PORT "2" +#define VOXL_ESC_DEFAULT_PORT "2" +#define VOXL2_IO_DEFAULT_PORT "2" \ No newline at end of file diff --git a/boards/modalai/voxl2/src/drivers/apps_sbus/CMakeLists.txt b/boards/modalai/voxl2/src/drivers/apps_sbus/CMakeLists.txt new file mode 100644 index 000000000000..1fd4c37c2120 --- /dev/null +++ b/boards/modalai/voxl2/src/drivers/apps_sbus/CMakeLists.txt @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_module( + MODULE drivers__apps_sbus + MAIN apps_sbus + COMPILE_FLAGS + -Wno-cast-align # TODO: fix and enable + SRCS + apps_sbus.cpp + ) diff --git a/boards/modalai/voxl2/src/drivers/apps_sbus/apps_sbus.cpp b/boards/modalai/voxl2/src/drivers/apps_sbus/apps_sbus.cpp new file mode 100644 index 000000000000..d708fea0292c --- /dev/null +++ b/boards/modalai/voxl2/src/drivers/apps_sbus/apps_sbus.cpp @@ -0,0 +1,453 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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 +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "protocol.h" + +extern "C" { __EXPORT int apps_sbus_main(int argc, char *argv[]); } + +namespace apps_sbus +{ + +std::string _port = ""; +int _uart_fd = -1; +IOPacket _packet; +bool _initialized = false; +bool _is_running = false; +uint64_t _rc_last_valid; ///< last valid timestamp +uint16_t _rc_valid_update_count = 0; + +static px4_task_t _task_handle = -1; + +uORB::PublicationMulti _rc_pub{ORB_ID(input_rc)}; + +int bus_exchange(IOPacket *packet) +{ + int ret = 0; + int read_retries = 3; + int read_succeeded = 0; + int packet_size = sizeof(IOPacket); + + tcflush(_uart_fd, TCIOFLUSH); + + ret = ::write(_uart_fd, packet, packet_size); + + usleep(100); + + struct pollfd fd_monitor; + fd_monitor.fd = _uart_fd; + fd_monitor.events = POLLIN; + + while (read_retries) { + (void) ::poll(&fd_monitor, 1, 1000); + ret = ::read(_uart_fd, packet, sizeof(IOPacket)); + + if (ret) { + // PX4_INFO("Read %d bytes", ret); + + /* Check CRC */ + uint8_t crc = packet->crc; + packet->crc = 0; + + if (crc != crc_packet(packet)) { + PX4_ERR("PX4IO packet CRC error"); + return -EIO; + + } else if (PKT_CODE(*packet) == PKT_CODE_CORRUPT) { + PX4_ERR("PX4IO packet corruption"); + return -EIO; + + } else { + read_succeeded = 1; + break; + } + } + + PX4_ERR("Read attempt %d failed", read_retries); + read_retries--; + } + + + if (! read_succeeded) { + return -EIO; + } + + return 0; +} + +int io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) +{ + /* range check the transfer */ + // if (num_values > ((_max_transfer) / sizeof(*values))) { + // PX4_ERR("io_reg_get: too many registers (%u, max %u)", num_values, _max_transfer / 2); + // return -1; + // } + + // int ret = _interface->read((page << 8) | offset, reinterpret_cast(values), num_values); + int ret = 0; + + _packet.count_code = num_values | PKT_CODE_READ; + _packet.page = page; + _packet.offset = offset; + + _packet.crc = 0; + _packet.crc = crc_packet(&_packet); + + ret = bus_exchange(&_packet); + + if (ret != 0) { + // PX4_ERR("px4io io_reg_get(%hhu,%hhu,%u): data error %d", page, offset, num_values, ret); + return -1; + } + + memcpy(values, &_packet.regs[0], num_values * 2); + + return OK; +} + +uint32_t io_reg_get(uint8_t page, uint8_t offset) +{ + uint16_t value; + + if (io_reg_get(page, offset, &value, 1) != OK) { + // Registers are only 16 bit so any value over 0xFFFF can signal a fault + return 0xFFFFFFFF; + } + + return value; +} + +int initialize() +{ + if (_initialized) { + // Already successfully initialized + return 0; + } + + if (_uart_fd < 0) { + _uart_fd = open("/dev/ttyHS1", O_RDWR | O_NONBLOCK); + + if (_uart_fd < 0) { + PX4_ERR("Open failed in %s", __FUNCTION__); + return -1; + + } else { + PX4_INFO("serial port fd %d", _uart_fd); + } + + // Configuration copied from dsm_config + struct termios uart_config; + + int termios_state; + + /* fill the struct for the new configuration */ + tcgetattr(_uart_fd, &uart_config); + + /* properly configure the terminal (see also https://en.wikibooks.org/wiki/Serial_Programming/termios ) */ + + // + // Input flags - Turn off input processing + // + // convert break to null byte, no CR to NL translation, + // no NL to CR translation, don't mark parity errors or breaks + // no input parity check, don't strip high bit off, + // no XON/XOFF software flow control + // + uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | + INLCR | PARMRK | INPCK | ISTRIP | IXON); + // + // Output flags - Turn off output processing + // + // no CR to NL translation, no NL to CR-NL translation, + // no NL to CR translation, no column 0 CR suppression, + // no Ctrl-D suppression, no fill characters, no case mapping, + // no local output processing + // + // config.c_oflag &= ~(OCRNL | ONLCR | ONLRET | + // ONOCR | ONOEOT| OFILL | OLCUC | OPOST); + uart_config.c_oflag = 0; + + // + // No line processing + // + // echo off, echo newline off, canonical mode off, + // extended input processing off, signal chars off + // + uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG); + + /* no parity, one stop bit, disable flow control */ + uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS); + + /* set baud rate */ + if ((termios_state = cfsetispeed(&uart_config, B921600)) < 0) { + PX4_ERR("ERR: %d (cfsetispeed)", termios_state); + return -1; + } + + if ((termios_state = cfsetospeed(&uart_config, B921600)) < 0) { + PX4_ERR("ERR: %d (cfsetospeed)", termios_state); + return -1; + } + + if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) { + PX4_ERR("ERR: %d (tcsetattr)", termios_state); + return -1; + } + + } + + if (_uart_fd < 0) { + PX4_ERR("Open failed in %s", __FUNCTION__); + return -1; + } + + // Verify connectivity and version number + unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); + + if (protocol != PX4IO_PROTOCOL_VERSION) { + PX4_ERR("apps_sbus version error: %u", protocol); + _uart_fd = -1; + return -1; + } + + _initialized = true; + + return 0; +} + +void apps_sbus_task() +{ + + uint16_t status_regs[2] {}; + input_rc_s rc_val; + const unsigned prolog = (PX4IO_P_RAW_RC_BASE - PX4IO_P_RAW_RC_COUNT); + uint16_t rc_regs[input_rc_s::RC_INPUT_MAX_CHANNELS + prolog]; + uint32_t channel_count = 0; + + _is_running = true; + + while (true) { + + usleep(20000); // Update every 20ms + + memset(&rc_val, 0, sizeof(input_rc_s)); + + if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, &status_regs[0], + sizeof(status_regs) / sizeof(status_regs[0])) == OK) { + // PX4_INFO("apps_sbus status 0x%.4x", status_regs[0]); + // PX4_INFO("apps_sbus alarms 0x%.4x", status_regs[1]); + } else { + // PX4_ERR("Failed to read status / alarm registers"); + continue; + } + + /* fetch values from IO */ + + // When starting the RC flag will not be okay if the receiver isn't + // getting a signal from the transmitter. Once it does, then this flag + // will say okay even if later the signal is lost. + if (!(status_regs[0] & PX4IO_P_STATUS_FLAGS_RC_OK)) { + // PX4_INFO("RC lost status flag set"); + rc_val.rc_lost = true; + + } else { + // PX4_INFO("RC lost status flag is not set"); + rc_val.rc_lost = false; + } + + if (status_regs[0] & PX4IO_P_STATUS_FLAGS_RC_SBUS) { + rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SBUS; + // PX4_INFO("Got valid SBUS"); + + } else { + rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_UNKNOWN; + // PX4_INFO("SBUS not valid"); + } + + rc_val.timestamp = hrt_absolute_time(); + + // No point in reading the registers if we haven't acquired a transmitter signal yet + if (! rc_val.rc_lost) { + if (io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, &rc_regs[0], + sizeof(rc_regs) / sizeof(rc_regs[0])) != OK) { + // PX4_ERR("Failed to read RC registers"); + continue; + // } else { + // PX4_INFO("Successfully read RC registers"); + // PX4_INFO("Prolog: %u 0x%.4x 0x%.4x 0x%.4x 0x%.4x 0x%.4x", + // rc_regs[0], rc_regs[1], rc_regs[2], rc_regs[3], rc_regs[4], rc_regs[5]); + } + + channel_count = rc_regs[PX4IO_P_RAW_RC_COUNT]; + + // const uint16_t rc_valid_update_count = rc_regs[PX4IO_P_RAW_FRAME_COUNT]; + // const bool rc_updated = (rc_valid_update_count != _rc_valid_update_count); + // + // if (!rc_updated) { + // PX4_INFO("Didn't get an RC update indication. %u %u", rc_valid_update_count, _rc_valid_update_count); + // continue; + // } + // + // _rc_valid_update_count = rc_valid_update_count; + // + // PX4_INFO("Got an RC update indication"); + + /* limit the channel count */ + if (channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) { + // PX4_INFO("Got %u for channel count. Limiting to 18", channel_count); + channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS; + } + + rc_val.channel_count = channel_count; + // PX4_INFO("RC channel count: %u", rc_val.channel_count); + + // rc_val.rc_ppm_frame_length = rc_regs[PX4IO_P_RAW_RC_DATA]; + rc_val.rc_ppm_frame_length = 0; + + rc_val.rc_failsafe = (rc_regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + // rc_val.rc_lost = !(rc_regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_RC_OK); + rc_val.rc_lost = rc_val.rc_failsafe; + rc_val.rc_lost_frame_count = rc_regs[PX4IO_P_RAW_LOST_FRAME_COUNT]; + rc_val.rc_total_frame_count = rc_regs[PX4IO_P_RAW_FRAME_COUNT]; + + if (!rc_val.rc_lost && !rc_val.rc_failsafe) { + _rc_last_valid = rc_val.timestamp; + rc_val.rssi = rc_regs[PX4IO_P_RAW_RC_NRSSI]; + rc_val.link_quality = rc_regs[PX4IO_P_RAW_RC_NRSSI]; + + /* last thing set are the actual channel values as 16 bit values */ + for (unsigned i = 0; i < channel_count; i++) { + rc_val.values[i] = rc_regs[prolog + i]; + // PX4_INFO("RC channel %u: %.4u", i, rc_val.values[i]); + } + + /* zero the remaining fields */ + for (unsigned i = channel_count; i < (sizeof(rc_val.values) / sizeof(rc_val.values[0])); i++) { + rc_val.values[i] = 0; + } + } + + rc_val.timestamp_last_signal = _rc_last_valid; + } + + _rc_pub.publish(rc_val); + + } +} + +int start(int argc, char *argv[]) +{ + + int ch; + int myoptind = 1; + const char *myoptarg = nullptr; + + while ((ch = px4_getopt(argc, argv, "p:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'p': + _port = myoptarg; + PX4_INFO("Setting port to %s", _port.c_str()); + break; + + default: + break; + } + } + + if (! _initialized) { + if (initialize()) { + return -1; + } + } + + if (_is_running) { + PX4_WARN("Already started"); + return 0; + } + + _task_handle = px4_task_spawn_cmd("apps_sbus_main", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2000, + (px4_main_t) &apps_sbus_task, + (char *const *)argv); + + if (_task_handle < 0) { + PX4_ERR("task start failed"); + return -1; + } + + return 0; +} + +void +usage() +{ + PX4_INFO("Usage: apps_sbus start [options]"); + PX4_INFO("Options: -p uart port number"); +} + +} // End namespance apps_sbus + +int apps_sbus_main(int argc, char *argv[]) +{ + int myoptind = 1; + + if (argc <= 1) { + apps_sbus::usage(); + return -1; + } + + const char *verb = argv[myoptind]; + + if (!strcmp(verb, "start")) { + return apps_sbus::start(argc - 1, argv + 1); + + } else { + apps_sbus::usage(); + return -1; + } + + return 0; +} diff --git a/boards/modalai/voxl2/src/drivers/apps_sbus/protocol.h b/boards/modalai/voxl2/src/drivers/apps_sbus/protocol.h new file mode 100644 index 000000000000..3cfcee2d1211 --- /dev/null +++ b/boards/modalai/voxl2/src/drivers/apps_sbus/protocol.h @@ -0,0 +1,405 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2017 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +/** + * @file protocol.h + * + * PX4IO interface protocol. + * + * @author Lorenz Meier + * + * Communication is performed via writes to and reads from 16-bit virtual + * registers organised into pages of 255 registers each. + * + * The first two bytes of each write select a page and offset address + * respectively. Subsequent reads and writes increment the offset within + * the page. + * + * Some pages are read- or write-only. + * + * Note that some pages may permit offset values greater than 255, which + * can only be achieved by long writes. The offset does not wrap. + * + * Writes to unimplemented registers are ignored. Reads from unimplemented + * registers return undefined values. + * + * As convention, values that would be floating point in other parts of + * the PX4 system are expressed as signed integer values scaled by 10000, + * e.g. control values range from -10000..10000. Use the REG_TO_SIGNED and + * SIGNED_TO_REG macros to convert between register representation and + * the signed version, and REG_TO_FLOAT/FLOAT_TO_REG to convert to float. + * + * Note that the implementation of readable pages prefers registers within + * readable pages to be densely packed. Page numbers do not need to be + * packed. + * + * Definitions marked [1] are only valid on PX4IOv1 boards. Likewise, + * [2] denotes definitions specific to the PX4IOv2 board. + */ + +/* Per C, this is safe for all 2's complement systems */ +#define REG_TO_SIGNED(_reg) ((int16_t)(_reg)) +#define SIGNED_TO_REG(_signed) ((uint16_t)(_signed)) + +#define REG_TO_FLOAT(_reg) ((float)REG_TO_SIGNED(_reg) / 10000.0f) +#define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)floorf((_float + 0.00005f) * 10000.0f)) + +#define REG_TO_BOOL(_reg) ((bool)(_reg)) + +#define PX4IO_PROTOCOL_VERSION 4 + +/* maximum allowable sizes on this protocol version */ +#define PX4IO_PROTOCOL_MAX_CONTROL_COUNT 8 /**< The protocol does not support more than set here, individual units might support less - see PX4IO_P_CONFIG_CONTROL_COUNT */ + +/* static configuration page */ +#define PX4IO_PAGE_CONFIG 0 +#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* PX4IO_PROTOCOL_VERSION */ +#define PX4IO_P_CONFIG_HARDWARE_VERSION 1 /* magic numbers TBD */ +#define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */ +#define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum I2C transfer size */ +#define PX4IO_P_CONFIG_CONTROL_COUNT 4 /* hardcoded max control count supported */ +#define PX4IO_P_CONFIG_ACTUATOR_COUNT 5 /* hardcoded max actuator output count */ +#define PX4IO_P_CONFIG_RC_INPUT_COUNT 6 /* hardcoded max R/C input count supported */ +#define PX4IO_P_CONFIG_ADC_INPUT_COUNT 7 /* hardcoded max ADC inputs */ +#define PX4IO_P_CONFIG_RELAY_COUNT 8 /* hardcoded # of relay outputs */ +#define PX4IO_MAX_TRANSFER_LEN 64 + +/* dynamic status page */ +#define PX4IO_PAGE_STATUS 1 +#define PX4IO_P_STATUS_FREEMEM 0 +#define PX4IO_P_STATUS_CPULOAD 1 + +#define PX4IO_P_STATUS_FLAGS 2 /* monitoring flags */ +#define PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED (1 << 0) /* arm-ok and locally armed */ +#define PX4IO_P_STATUS_FLAGS_OVERRIDE (1 << 1) /* in manual override */ +#define PX4IO_P_STATUS_FLAGS_RC_OK (1 << 2) /* RC input is valid */ +#define PX4IO_P_STATUS_FLAGS_RC_PPM (1 << 3) /* PPM input is valid */ +#define PX4IO_P_STATUS_FLAGS_RC_DSM (1 << 4) /* DSM input is valid */ +#define PX4IO_P_STATUS_FLAGS_RC_SBUS (1 << 5) /* SBUS input is valid */ +#define PX4IO_P_STATUS_FLAGS_FMU_OK (1 << 6) /* controls from FMU are valid */ +#define PX4IO_P_STATUS_FLAGS_RAW_PWM (1 << 7) /* raw PWM from FMU is bypassing the mixer */ +#define PX4IO_P_STATUS_FLAGS_MIXER_OK (1 << 8) /* mixer is OK */ +#define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 9) /* the arming state between IO and FMU is in sync */ +#define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */ +#define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */ +#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 12) /* safety is off */ +#define PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED (1 << 13) /* FMU was initialized and OK once */ +#define PX4IO_P_STATUS_FLAGS_RC_ST24 (1 << 14) /* ST24 input is valid */ +#define PX4IO_P_STATUS_FLAGS_RC_SUMD (1 << 15) /* SUMD input is valid */ + +#define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */ +#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* [1] VBatt is very close to regulator dropout */ +#define PX4IO_P_STATUS_ALARMS_TEMPERATURE (1 << 1) /* board temperature is high */ +#define PX4IO_P_STATUS_ALARMS_SERVO_CURRENT (1 << 2) /* [1] servo current limit was exceeded */ +#define PX4IO_P_STATUS_ALARMS_ACC_CURRENT (1 << 3) /* [1] accessory current limit was exceeded */ +#define PX4IO_P_STATUS_ALARMS_FMU_LOST (1 << 4) /* timed out waiting for controls from FMU */ +#define PX4IO_P_STATUS_ALARMS_RC_LOST (1 << 5) /* timed out waiting for RC input */ +#define PX4IO_P_STATUS_ALARMS_PWM_ERROR (1 << 6) /* PWM configuration or output was bad */ +#define PX4IO_P_STATUS_ALARMS_VSERVO_FAULT (1 << 7) /* [2] VServo was out of the valid range (2.5 - 5.5 V) */ + +#define PX4IO_P_STATUS_VSERVO 6 /* [2] servo rail voltage in mV */ +#define PX4IO_P_STATUS_VRSSI 7 /* [2] RSSI voltage */ +#define PX4IO_P_STATUS_PRSSI 8 /* [2] RSSI PWM value */ + +#define PX4IO_P_STATUS_MIXER 9 /* mixer actuator limit flags */ + +/* array of post-mix actuator outputs, -10000..10000 */ +#define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* array of PWM servo output values, microseconds */ +#define PX4IO_PAGE_SERVOS 3 /* 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* array of raw RC input values, microseconds */ +#define PX4IO_PAGE_RAW_RC_INPUT 4 +#define PX4IO_P_RAW_RC_COUNT 0 /* number of valid channels */ +#define PX4IO_P_RAW_RC_FLAGS 1 /* RC detail status flags */ +#define PX4IO_P_RAW_RC_FLAGS_FRAME_DROP (1 << 0) /* single frame drop */ +#define PX4IO_P_RAW_RC_FLAGS_FAILSAFE (1 << 1) /* receiver is in failsafe mode */ +#define PX4IO_P_RAW_RC_FLAGS_RC_DSM11 (1 << 2) /* DSM decoding is 11 bit mode */ +#define PX4IO_P_RAW_RC_FLAGS_MAPPING_OK (1 << 3) /* Channel mapping is ok */ +#define PX4IO_P_RAW_RC_FLAGS_RC_OK (1 << 4) /* RC reception ok */ + +#define PX4IO_P_RAW_RC_NRSSI 2 /* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */ +#define PX4IO_P_RAW_RC_DATA 3 /* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */ +#define PX4IO_P_RAW_FRAME_COUNT 4 /* Number of total received frames (wrapping counter) */ +#define PX4IO_P_RAW_LOST_FRAME_COUNT 5 /* Number of total dropped frames (wrapping counter) */ +#define PX4IO_P_RAW_RC_BASE 6 /* CONFIG_RC_INPUT_COUNT channels from here */ + +/* array of scaled RC input values, -10000..10000 */ +#define PX4IO_PAGE_RC_INPUT 5 +#define PX4IO_P_RC_VALID 0 /* bitmask of valid controls */ +#define PX4IO_P_RC_BASE 1 /* CONFIG_RC_INPUT_COUNT controls from here */ + +/* array of raw ADC values */ +#define PX4IO_PAGE_RAW_ADC_INPUT 6 /* 0..CONFIG_ADC_INPUT_COUNT-1 */ + +/* PWM servo information */ +#define PX4IO_PAGE_PWM_INFO 7 +#define PX4IO_RATE_MAP_BASE 0 /* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */ + +/* setup page */ +#define PX4IO_PAGE_SETUP 50 +#define PX4IO_P_SETUP_FEATURES 0 +#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT (1 << 0) /**< enable S.Bus v1 output */ +#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT (1 << 1) /**< enable S.Bus v2 output */ +#define PX4IO_P_SETUP_FEATURES_PWM_RSSI (1 << 2) /**< enable PWM RSSI parsing */ +#define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 3) /**< enable ADC RSSI parsing */ + +#define PX4IO_P_SETUP_ARMING 1 /* arming controls */ +#define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */ +#define PX4IO_P_SETUP_ARMING_FMU_ARMED (1 << 1) /* FMU is already armed */ +#define PX4IO_P_SETUP_ARMING_FMU_PREARMED (1 << 2) /* FMU is already prearmed */ +#define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK (1 << 3) /* OK to switch to manual override via override RC channel */ +#define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM (1 << 4) /* use custom failsafe values, not 0 values of mixer */ +#define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 5) /* OK to try in-air restart */ +#define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE (1 << 6) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */ +#define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED (1 << 7) /* Disable the IO-internal evaluation of the RC */ +#define PX4IO_P_SETUP_ARMING_LOCKDOWN (1 << 8) /* If set, the system operates normally, but won't actuate any servos */ +#define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE (1 << 9) /* If set, the system will always output the failsafe values */ +#define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE (1 << 10) /* If set, the system will never return from a failsafe, but remain in failsafe once triggered. */ +#define PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE (1 << 11) /* If set then on FMU failure override is immediate. Othewise it waits for the mode switch to go past the override thrshold */ + +#define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ +#define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ +#define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */ +#define PX4IO_P_SETUP_RELAYS_PAD 5 + +#define PX4IO_P_SETUP_VSERVO_SCALE 6 /* hardware rev [2] servo voltage correction factor (float) */ +#define PX4IO_P_SETUP_DSM 7 /* DSM bind state */ +enum { /* DSM bind states */ + dsm_bind_power_down = 0, + dsm_bind_power_up, + dsm_bind_set_rx_out, + dsm_bind_send_pulses, + dsm_bind_reinit_uart +}; +/* 8 */ +#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ + +#define PX4IO_P_SETUP_REBOOT_BL 10 /* reboot IO into bootloader */ +#define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */ + +#define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */ +/* storage space of 12 occupied by CRC */ +#define PX4IO_P_SETUP_FORCE_SAFETY_OFF 12 /* force safety switch into + 'armed' (PWM enabled) state - this is a non-data write and + hence index 12 can safely be used. */ +#define PX4IO_P_SETUP_RC_THR_FAILSAFE_US 13 /**< the throttle failsafe pulse length in microseconds */ + +#define PX4IO_P_SETUP_FORCE_SAFETY_ON 14 /* force safety switch into 'disarmed' (PWM disabled state) */ +#define PX4IO_FORCE_SAFETY_MAGIC 22027 /* required argument for force safety (random) */ + +#define PX4IO_P_SETUP_PWM_REVERSE 15 /**< Bitmask to reverse PWM channels 1-8 */ +#define PX4IO_P_SETUP_TRIM_ROLL 16 /**< Roll trim, in actuator units */ +#define PX4IO_P_SETUP_TRIM_PITCH 17 /**< Pitch trim, in actuator units */ +#define PX4IO_P_SETUP_TRIM_YAW 18 /**< Yaw trim, in actuator units */ +#define PX4IO_P_SETUP_SCALE_ROLL 19 /**< Roll scale, in actuator units */ +#define PX4IO_P_SETUP_SCALE_PITCH 20 /**< Pitch scale, in actuator units */ +#define PX4IO_P_SETUP_SCALE_YAW 21 /**< Yaw scale, in actuator units */ + +#define PX4IO_P_SETUP_SBUS_RATE 22 /**< frame rate of SBUS1 output in Hz */ + +#define PX4IO_P_SETUP_MOTOR_SLEW_MAX 24 /**< max motor slew rate */ + +#define PX4IO_P_SETUP_THR_MDL_FAC 25 /**< factor for modelling motor control signal output to static thrust relationship */ + +#define PX4IO_P_SETUP_THERMAL 26 /**< thermal management */ + +#define PX4IO_P_SETUP_AIRMODE 27 /**< air-mode */ + +#define PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION 28 /**< flight termination; false if the circuit breaker (CBRK_FLIGHTTERM) is set */ + +#define PX4IO_THERMAL_IGNORE UINT16_MAX +#define PX4IO_THERMAL_OFF 0 +#define PX4IO_THERMAL_FULL 10000 + +/* autopilot control values, -10000..10000 */ +#define PX4IO_PAGE_CONTROLS 51 /**< actuator control groups, one after the other, 8 wide */ +#define PX4IO_P_CONTROLS_GROUP_0 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 0) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ +#define PX4IO_P_CONTROLS_GROUP_1 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 1) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ +#define PX4IO_P_CONTROLS_GROUP_2 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 2) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ +#define PX4IO_P_CONTROLS_GROUP_3 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 3) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ + +#define PX4IO_P_CONTROLS_GROUP_VALID 64 +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0 (1 << 0) /**< group 0 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1 (1 << 1) /**< group 1 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2 (1 << 2) /**< group 2 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3 (1 << 3) /**< group 3 is valid / received */ + +/* raw text load to the mixer parser - ignores offset */ +#define PX4IO_PAGE_MIXERLOAD 52 + +/* R/C channel config */ +#define PX4IO_PAGE_RC_CONFIG 53 /**< R/C input configuration */ +#define PX4IO_P_RC_CONFIG_MIN 0 /**< lowest input value */ +#define PX4IO_P_RC_CONFIG_CENTER 1 /**< center input value */ +#define PX4IO_P_RC_CONFIG_MAX 2 /**< highest input value */ +#define PX4IO_P_RC_CONFIG_DEADZONE 3 /**< band around center that is ignored */ +#define PX4IO_P_RC_CONFIG_ASSIGNMENT 4 /**< mapped input value */ +#define PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH 100 /**< magic value for mode switch */ +#define PX4IO_P_RC_CONFIG_OPTIONS 5 /**< channel options bitmask */ +#define PX4IO_P_RC_CONFIG_OPTIONS_ENABLED (1 << 0) +#define PX4IO_P_RC_CONFIG_OPTIONS_REVERSE (1 << 1) +#define PX4IO_P_RC_CONFIG_STRIDE 6 /**< spacing between channel config data */ + +/* PWM output - overrides mixer */ +#define PX4IO_PAGE_DIRECT_PWM 54 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* PWM failsafe values - zero disables the output */ +#define PX4IO_PAGE_FAILSAFE_PWM 55 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* PWM failsafe values - zero disables the output */ +#define PX4IO_PAGE_SENSORS 56 /**< Sensors connected to PX4IO */ +#define PX4IO_P_SENSORS_ALTITUDE 0 /**< Altitude of an external sensor (HoTT or S.BUS2) */ + +/* Debug and test page - not used in normal operation */ +#define PX4IO_PAGE_TEST 127 +#define PX4IO_P_TEST_LED 0 /**< set the amber LED on/off */ + +/* PWM minimum values for certain ESCs */ +#define PX4IO_PAGE_CONTROL_MIN_PWM 106 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* PWM maximum values for certain ESCs */ +#define PX4IO_PAGE_CONTROL_MAX_PWM 107 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* PWM mtrim values for central position */ +#define PX4IO_PAGE_CONTROL_TRIM_PWM 108 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* PWM disarmed values that are active, even when SAFETY_SAFE */ +#define PX4IO_PAGE_DISARMED_PWM 109 /* 0..CONFIG_ACTUATOR_COUNT-1 */ + +/** + * As-needed mixer data upload. + * + * This message adds text to the mixer text buffer; the text + * buffer is drained as the definitions are consumed. + */ +#pragma pack(push, 1) +struct px4io_mixdata { + uint16_t f2i_mixer_magic; +#define F2I_MIXER_MAGIC 0x6d74 + + uint8_t action; +#define F2I_MIXER_ACTION_RESET 0 +#define F2I_MIXER_ACTION_APPEND 1 + + char text[0]; /* actual text size may vary */ +}; +#pragma pack(pop) + +/** + * Serial protocol encapsulation. + */ + +#define PKT_MAX_REGS 32 // by agreement w/FMU + +#pragma pack(push, 1) +struct IOPacket { + uint8_t count_code; + uint8_t crc; + uint8_t page; + uint8_t offset; + uint16_t regs[PKT_MAX_REGS]; +}; +#pragma pack(pop) + +#if (PX4IO_MAX_TRANSFER_LEN > PKT_MAX_REGS * 2) +#error The max transfer length of the IO protocol must not be larger than the IO packet size +#endif + +#define PKT_CODE_READ 0x00 /* FMU->IO read transaction */ +#define PKT_CODE_WRITE 0x40 /* FMU->IO write transaction */ +#define PKT_CODE_SUCCESS 0x00 /* IO->FMU success reply */ +#define PKT_CODE_CORRUPT 0x40 /* IO->FMU bad packet reply */ +#define PKT_CODE_ERROR 0x80 /* IO->FMU register op error reply */ + +#define PKT_CODE_MASK 0xc0 +#define PKT_COUNT_MASK 0x3f + +#define PKT_COUNT(_p) ((_p).count_code & PKT_COUNT_MASK) +#define PKT_CODE(_p) ((_p).count_code & PKT_CODE_MASK) +#define PKT_SIZE(_p) ((size_t)((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p)))) + +static const uint8_t crc8_tab[256] __attribute__((unused)) = { + 0x00, 0x07, 0x0E, 0x09, 0x1C, 0x1B, 0x12, 0x15, + 0x38, 0x3F, 0x36, 0x31, 0x24, 0x23, 0x2A, 0x2D, + 0x70, 0x77, 0x7E, 0x79, 0x6C, 0x6B, 0x62, 0x65, + 0x48, 0x4F, 0x46, 0x41, 0x54, 0x53, 0x5A, 0x5D, + 0xE0, 0xE7, 0xEE, 0xE9, 0xFC, 0xFB, 0xF2, 0xF5, + 0xD8, 0xDF, 0xD6, 0xD1, 0xC4, 0xC3, 0xCA, 0xCD, + 0x90, 0x97, 0x9E, 0x99, 0x8C, 0x8B, 0x82, 0x85, + 0xA8, 0xAF, 0xA6, 0xA1, 0xB4, 0xB3, 0xBA, 0xBD, + 0xC7, 0xC0, 0xC9, 0xCE, 0xDB, 0xDC, 0xD5, 0xD2, + 0xFF, 0xF8, 0xF1, 0xF6, 0xE3, 0xE4, 0xED, 0xEA, + 0xB7, 0xB0, 0xB9, 0xBE, 0xAB, 0xAC, 0xA5, 0xA2, + 0x8F, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9D, 0x9A, + 0x27, 0x20, 0x29, 0x2E, 0x3B, 0x3C, 0x35, 0x32, + 0x1F, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0D, 0x0A, + 0x57, 0x50, 0x59, 0x5E, 0x4B, 0x4C, 0x45, 0x42, + 0x6F, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7D, 0x7A, + 0x89, 0x8E, 0x87, 0x80, 0x95, 0x92, 0x9B, 0x9C, + 0xB1, 0xB6, 0xBF, 0xB8, 0xAD, 0xAA, 0xA3, 0xA4, + 0xF9, 0xFE, 0xF7, 0xF0, 0xE5, 0xE2, 0xEB, 0xEC, + 0xC1, 0xC6, 0xCF, 0xC8, 0xDD, 0xDA, 0xD3, 0xD4, + 0x69, 0x6E, 0x67, 0x60, 0x75, 0x72, 0x7B, 0x7C, + 0x51, 0x56, 0x5F, 0x58, 0x4D, 0x4A, 0x43, 0x44, + 0x19, 0x1E, 0x17, 0x10, 0x05, 0x02, 0x0B, 0x0C, + 0x21, 0x26, 0x2F, 0x28, 0x3D, 0x3A, 0x33, 0x34, + 0x4E, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5C, 0x5B, + 0x76, 0x71, 0x78, 0x7F, 0x6A, 0x6D, 0x64, 0x63, + 0x3E, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2C, 0x2B, + 0x06, 0x01, 0x08, 0x0F, 0x1A, 0x1D, 0x14, 0x13, + 0xAE, 0xA9, 0xA0, 0xA7, 0xB2, 0xB5, 0xBC, 0xBB, + 0x96, 0x91, 0x98, 0x9F, 0x8A, 0x8D, 0x84, 0x83, + 0xDE, 0xD9, 0xD0, 0xD7, 0xC2, 0xC5, 0xCC, 0xCB, + 0xE6, 0xE1, 0xE8, 0xEF, 0xFA, 0xFD, 0xF4, 0xF3 +}; + +static uint8_t crc_packet(struct IOPacket *pkt) __attribute__((unused)); +static uint8_t +crc_packet(struct IOPacket *pkt) +{ + uint8_t *end = (uint8_t *)(&pkt->regs[PKT_COUNT(*pkt)]); + uint8_t *p = (uint8_t *)pkt; + uint8_t c = 0; + + while (p < end) { + c = crc8_tab[c ^ * (p++)]; + } + + return c; +} diff --git a/boards/modalai/voxl2/target/voxl-px4 b/boards/modalai/voxl2/target/voxl-px4 index 38560b47a091..3ce4309ee2ba 100755 --- a/boards/modalai/voxl2/target/voxl-px4 +++ b/boards/modalai/voxl2/target/voxl-px4 @@ -4,7 +4,9 @@ CONFIG_FILE="/etc/modalai/voxl-px4.conf" GPS=NONE RC=SPEKTRUM -IMU_ROTATION=NONE +ESC=VOXL_ESC +POWER_MANAGER=VOXLPM +DISTANCE_SENSOR=NONE OSD=DISABLE DAEMON_MODE=DISABLE SENSOR_CAL=ACTUAL @@ -39,9 +41,8 @@ fi print_usage() { echo -e "\nUsage: voxl-px4 [-b (Specify Holybro GPS unit)]" echo " [-c delete configuration file and exit]" - echo " [-d start px4 in daemon mode]" + echo " [-d start px4 without daemon mode]" echo " [-f (Use fake rc input instead of from a real transmitter)]" - echo " [-i (Set IMU_ROTATION to ROTATION_YAW_180)]" echo " [-m (Specify Matek GPS unit)]" echo " [-o (Start OSD module on the apps processor)]" echo " [-r (Specify TBS Crossfire RC receiver, MAVLINK)]" @@ -56,7 +57,9 @@ print_config_settings(){ echo -e "\n*************************" echo "GPS=$GPS" echo "RC=$RC" - echo "IMU_ROTATION=$IMU_ROTATION" + echo "ESC=$ESC" + echo "POWER MANAGER=$POWER_MANAGER" + echo "DISTANCE SENSOR=$DISTANCE_SENSOR" echo "OSD=$OSD" echo "DAEMON_MODE=$DAEMON_MODE" echo "SENSOR_CAL=$SENSOR_CAL" @@ -68,7 +71,7 @@ print_config_settings(){ echo -e "*************************\n" } -while getopts "bcdhifmorwz" flag +while getopts "bcdhfmorwz" flag do case "${flag}" in b) @@ -83,16 +86,12 @@ do exit 0 ;; d) - echo "[INFO] Enabling daemon mode" - DAEMON_MODE=ENABLE + echo "[INFO] Disabling daemon mode" + DAEMON_MODE=DISABLE ;; h) print_usage ;; - i) - echo "[INFO] Setting IMU_ROTATION to 4: ROTATION_YAW_180" - IMU_ROTATION=4 - ;; f) echo "[INFO] Setting RC to FAKE_RC_INPUT" RC=FAKE_RC_INPUT @@ -144,4 +143,5 @@ fi print_config_settings -GPS=$GPS RC=$RC OSD=$OSD IMU_ROTATION=$IMU_ROTATION EXTRA_STEPS=$EXTRA_STEPS px4 $DAEMON -s /usr/bin/voxl-px4-start +GPS=$GPS RC=$RC ESC=$ESC POWER_MANAGER=$POWER_MANAGER DISTANCE_SENSOR=$DISTANCE_SENSOR \ +OSD=$OSD EXTRA_STEPS=$EXTRA_STEPS px4 $DAEMON -s /usr/bin/voxl-px4-start diff --git a/boards/modalai/voxl2/target/voxl-px4-hitl b/boards/modalai/voxl2/target/voxl-px4-hitl new file mode 100755 index 000000000000..c2b2f4158317 --- /dev/null +++ b/boards/modalai/voxl2/target/voxl-px4-hitl @@ -0,0 +1,39 @@ +#!/bin/bash + +# Make sure that the SLPI DSP test signature is there otherwise px4 cannot run +# on the DSP +if /bin/ls /usr/lib/rfsa/adsp/testsig-*.so &> /dev/null; then + /bin/echo "Found DSP signature file" +else + /bin/echo "[WARNING] Could not find DSP signature file" + # Look for the DSP signature generation script + if [ -f /share/modalai/qrb5165-slpi-test-sig/generate-test-sig.sh ]; then + /bin/echo "[INFO] Attempting to generate the DSP signature file" + # Automatically generate the test signature so that px4 can run on SLPI DSP + /share/modalai/qrb5165-slpi-test-sig/generate-test-sig.sh + else + /bin/echo "[ERROR] Could not find the DSP signature file generation script" + exit 0 + fi +fi + +print_usage() { + echo -e "\nUsage: voxl-px4-hitl" + echo " [-h (show help)]" + + exit 1 +} + +while getopts "h" flag +do + case "${flag}" in + h) + print_usage + ;; + *) + print_usage + ;; + esac +done + +px4 -s /usr/bin/voxl-px4-hitl-start diff --git a/boards/modalai/voxl2/target/voxl-px4-hitl-set-default-parameters.config b/boards/modalai/voxl2/target/voxl-px4-hitl-set-default-parameters.config new file mode 100755 index 000000000000..f4455955f78b --- /dev/null +++ b/boards/modalai/voxl2/target/voxl-px4-hitl-set-default-parameters.config @@ -0,0 +1,111 @@ +# Param setting and declaring file + +param select /data/px4/param/hitl_parameters + +# Sys config parameters +param set SYS_AUTOSTART 1001 +param set SYS_HITL 1 + +# Make sure we are running at 800Hz on IMU +param set IMU_GYRO_RATEMAX 800 + +# Some parameters for control allocation +param set CA_ROTOR_COUNT 4 +param set CA_AIRFRAME 0 +param set CA_ROTOR_COUNT 4 +param set CA_ROTOR0_PX 0.15 +param set CA_ROTOR0_PY 0.25 +param set CA_ROTOR1_PX -0.15 +param set CA_ROTOR1_PY -0.19 +param set CA_ROTOR2_PX 0.15 +param set CA_ROTOR2_PY -0.25 +param set CA_ROTOR2_KM -0.05 +param set CA_ROTOR3_PX -0.15 +param set CA_ROTOR3_PY 0.19 +param set CA_ROTOR3_KM -0.05 + +# Sensor calibration parameters +param set CAL_ACC0_ID 2490378 +param set CAL_ACC0_PRIO 50 +param set CAL_ACC0_XOFF -0.018255233764648438 +param set CAL_ACC0_XSCALE 1.000119328498840332 +param set CAL_ACC0_YOFF 0.097194194793701172 +param set CAL_ACC0_YSCALE 1.003928661346435547 +param set CAL_ACC0_ZOFF 0.081269264221191406 +param set CAL_ACC0_ZSCALE 0.992971897125244141 +param set CAL_ACC1_PRIO 50 +param set CAL_ACC2_PRIO 50 +param set CAL_ACC3_PRIO 50 +param set CAL_AIR_CMODEL 0 +param set CAL_AIR_TUBED_MM 1.500000000000000000 +param set CAL_AIR_TUBELEN 0.200000002980232239 +param set CAL_GYRO0_ID 2490378 +param set CAL_GYRO0_PRIO 50 +param set CAL_GYRO0_XOFF 0.013671654276549816 +param set CAL_GYRO0_YOFF -0.000422076700488105 +param set CAL_GYRO0_ZOFF -0.003227389883249998 +param set CAL_GYRO1_PRIO 50 +param set CAL_GYRO2_PRIO 50 +param set CAL_GYRO3_PRIO 50 +param set CAL_MAG0_ID 396809 +param set CAL_MAG0_PRIO 75 +param set CAL_MAG0_ROT 0 +param set CAL_MAG0_XODIAG -0.011825157329440117 +param set CAL_MAG0_XOFF -0.011212680488824844 +param set CAL_MAG0_XSCALE 1.001187443733215332 +param set CAL_MAG0_YODIAG 0.022539729252457619 +param set CAL_MAG0_YOFF -0.030884368345141411 +param set CAL_MAG0_YSCALE 0.940797865390777588 +param set CAL_MAG0_ZODIAG -0.006671304814517498 +param set CAL_MAG0_ZOFF -0.097350947558879852 +param set CAL_MAG0_ZSCALE 1.050295352935791016 +param set CAL_MAG1_PRIO 50 +param set CAL_MAG2_PRIO 50 +param set CAL_MAG3_PRIO 50 + +# Commander parameters +param set COM_CPU_MAX 0 +param set COM_DISARM_PRFLT -1 +param set COM_RC_IN_MODE 1 +param set NAV_RCL_ACT 1 +param set COM_FLIGHT_UUID 15 + +# EKF2 parameters +param set EKF2_ABL_LIM 0.8 +param set EKF2_EV_DELAY 5.0 +param set EKF2_IMU_POS_X 0.027 +param set EKF2_IMU_POS_Y 0.009 +param set EKF2_IMU_POS_Z -0.019 +param set EKF2_MAG_DECL 3.2 + +# Control allocator parameters for HIL +param set HIL_ACT_FUNC1 101 +param set HIL_ACT_FUNC2 102 +param set HIL_ACT_FUNC3 103 +param set HIL_ACT_FUNC4 104 +param set PWM_MAIN_FUNC1 101 +param set PWM_MAIN_FUNC2 102 +param set PWM_MAIN_FUNC3 103 +param set PWM_MAIN_FUNC4 104 + +# Mavlink parameters +param set MAV_TYPE 2 + +# Autotune parameters +param set MC_AT_EN 1 + +# RC Mapping parameters +param set RC_MAP_PITCH 2 +param set RC_MAP_ROLL 1 +param set RC_MAP_THROTTLE 3 +param set RC_MAP_YAW 4 +param set RC_MAP_FLTMODE 6 +param set RC_MAP_KILL_SW 7 + +# CBRK parameters +param set CBRK_SUPPLY_CHK 894281 + +# Landing parameters +param set LND_FLIGHT_T_LO 483791313 + +param save diff --git a/boards/modalai/voxl2/target/voxl-px4-hitl-start b/boards/modalai/voxl2/target/voxl-px4-hitl-start new file mode 100755 index 000000000000..45ef0b2ddf53 --- /dev/null +++ b/boards/modalai/voxl2/target/voxl-px4-hitl-start @@ -0,0 +1,102 @@ +#!/bin/sh +# PX4 commands need the 'px4-' prefix in bash. +# (px4-alias.sh is expected to be in the PATH) +. px4-alias.sh + +# Figure out what platform we are running on. +PLATFORM=`/usr/bin/voxl-platform 2> /dev/null` +RETURNCODE=$? +if [ $RETURNCODE -ne 0 ]; then + # If we couldn't get the platform from the voxl-platform utility then check + # /etc/version to see if there is an M0052 substring in the version string. If so, + # then we assume that we are on M0052. + VERSIONSTRING=$( /dev/null` +RETURNCODE=$? +if [ $RETURNCODE -ne 0 ]; then + # If we couldn't get the platform from the voxl-platform utility then check + # /etc/version to see if there is an M0052 substring in the version string. If so, + # then we assume that we are on M0052. + VERSIONSTRING=$( /* LEDs are driven with push open drain to support Anode to 5V or 3.3V */ -#define GPIO_nLED_RED /* PB11 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11) -#define GPIO_nLED_GREEN /* PB1 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN1) +#define GPIO_nLED_RED /* PB4 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN4) +#define GPIO_nLED_GREEN /* PB5 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN5) #define GPIO_nLED_BLUE /* PB3 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN3) +#define GPIO_LED_SAFETY /* PE6 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN6) + #define BOARD_HAS_CONTROL_STATUS_LEDS 1 #define BOARD_OVERLOAD_LED LED_RED #define BOARD_ARMED_STATE_LED LED_BLUE /* ADC channels */ #define PX4_ADC_GPIO \ - /* PA2 */ GPIO_ADC12_INP14, \ + /* PC4 */ GPIO_ADC12_INP4, \ /* PA3 */ GPIO_ADC12_INP15, \ /* PA4 */ GPIO_ADC12_INP18, \ - /* PC1 */ GPIO_ADC123_INP11 + /* PC0 */ GPIO_ADC123_INP10, \ + /* PC5 */ GPIO_ADC12_INP8, \ + /* PB0 */ GPIO_ADC12_INP9, \ + /* PB1 */ GPIO_ADC12_INP5 /* Define Channel numbers must match above GPIO pins */ -#define ADC_BATTERY_VOLTAGE_CHANNEL 14 /* PA2 BATT_VOLT_SENS */ +#define ADC_BATTERY_VOLTAGE_CHANNEL 4 /* PC4 BATT_VOLT_SENS */ #define ADC_BATTERY_CURRENT_CHANNEL 15 /* PA3 BATT_CURRENT_SENS */ #define ADC_SCALED_V5_CHANNEL 18 /* PA4 VDD_5V_SENS */ -#define ADC_RC_RSSI_CHANNEL 11 /* PC1 */ +#define ADC_RC_RSSI_CHANNEL 10 /* PC0 */ +#define ADC_AIRSPEED_VOLTAGE_CHANNEL 8 /* PC5 */ +#define ADC_AUX1_VOLTAGE_CHANNEL 9 /* PB0 */ +#define ADC_AUX2_VOLTAGE_CHANNEL 5 /* PB1 */ #define ADC_CHANNELS \ ((1 << ADC_BATTERY_VOLTAGE_CHANNEL) | \ (1 << ADC_BATTERY_CURRENT_CHANNEL) | \ (1 << ADC_SCALED_V5_CHANNEL) | \ - (1 << ADC_RC_RSSI_CHANNEL)) + (1 << ADC_RC_RSSI_CHANNEL) | \ + (1 << ADC_AIRSPEED_VOLTAGE_CHANNEL) | \ + (1 << ADC_AUX1_VOLTAGE_CHANNEL) | \ + (1 << ADC_AUX2_VOLTAGE_CHANNEL)) /* HW has to large of R termination on ADC todo:change when HW value is chosen */ #define BOARD_ADC_OPEN_CIRCUIT_V (5.6f) /* CAN Silence: Silent mode control */ -#define GPIO_CAN1_SILENT_S0 /* PF5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN5) -#define GPIO_CAN2_SILENT_S0 /* PF5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN5) +#define GPIO_CAN1_SILENT_S0 /* PF11 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN11) +#define GPIO_CAN2_SILENT_S0 /* PF12 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN12) /* PWM */ #define DIRECT_PWM_OUTPUT_CHANNELS 12 /* Power supply control and monitoring GPIOs */ -#define GPIO_nPOWER_IN_A /* PB5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) +#define GPIO_nPOWER_IN_A /* PC1 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1) #define GPIO_VDD_BRICK1_VALID GPIO_nPOWER_IN_A /* Brick 1 Is Chosen */ #define BOARD_NUMBER_BRICKS 1 #define GPIO_VDD_3V3_SPEKTRUM_POWER_EN /* PE4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4) +#define GPIO_LEVEL_SHIFTER_OE /* PC13 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) + /* Define True logic Power Control in arch agnostic form */ #define VDD_3V3_SPEKTRUM_POWER_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SPEKTRUM_POWER_EN, (!on_true)) #define READ_VDD_3V3_SPEKTRUM_POWER_EN() (px4_arch_gpioread(GPIO_VDD_3V3_SPEKTRUM_POWER_EN) == 0) /* Tone alarm output */ -#define TONE_ALARM_TIMER 2 /* timer 2 */ -#define TONE_ALARM_CHANNEL 1 /* PA15 TIM2_CH1 */ +#define TONE_ALARM_TIMER 16 /* timer 16 */ +#define TONE_ALARM_CHANNEL 1 /* PF6 TIM16_CH1 */ -#define GPIO_BUZZER_1 /* PA15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15) +#define GPIO_BUZZER_1 /* PF6 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN6) #define GPIO_TONE_ALARM_IDLE GPIO_BUZZER_1 -#define GPIO_TONE_ALARM GPIO_TIM2_CH1OUT_2 +#define GPIO_TONE_ALARM GPIO_TIM16_CH1OUT_2 /* USB OTG FS */ #define GPIO_OTGFS_VBUS /* PA9 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_100MHz|GPIO_PORTA|GPIO_PIN9) /* High-resolution timer */ #define HRT_TIMER 3 /* use timer3 for the HRT */ -#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel 1 */ +#define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 1 */ -#define HRT_PPM_CHANNEL /* T3C3 */ 3 /* use capture/compare channel 3 */ -#define GPIO_PPM_IN /* PB0 T3C3 */ GPIO_TIM3_CH3IN_1 +#define HRT_PPM_CHANNEL /* T3C2 */ 2 /* use capture/compare channel 2 */ +#define GPIO_PPM_IN /* PC7 T3C2 */ GPIO_TIM3_CH2IN_3 /* RC Serial port */ -#define RC_SERIAL_PORT "/dev/ttyS3" +#define RC_SERIAL_PORT "/dev/ttyS5" -#define GPIO_RSSI_IN /* PC1 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1) +#define GPIO_RSSI_IN /* PC0 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN0) /* Safety Switch: Enable the FMU to control it if there is no px4io fixme:This should be BOARD_SAFETY_LED(__ontrue) */ -#define GPIO_SAFETY_SWITCH_IN /* PC4 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4) -/* Enable the FMU to use the switch it if there is no px4io fixme:This should be BOARD_SAFTY_BUTTON() */ +#define GPIO_SAFETY_SWITCH_IN /* PA10 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN10) +/* Enable the FMU to use the switch it if there is no px4io fixme:This should be BOARD_SAFETY_BUTTON() */ #define GPIO_BTN_SAFETY GPIO_SAFETY_SWITCH_IN /* Enable the FMU to control it if there is no px4io */ +/* No PX4IO processor present */ +#define PX4_MFT_HW_SUPPORTED_PX4_MFT_PX4IO 0 + /* Power switch controls ******************************************************/ #define SPEKTRUM_POWER(_on_true) VDD_3V3_SPEKTRUM_POWER_EN(_on_true) /* * Board has a separate RC_IN * - * GPIO PPM_IN on PB0 T3CH3 + * GPIO PPM_IN on PC7 T3CH2 * SPEKTRUM_RX (it's TX or RX in Bind) on UART6 PC7 * Inversion is possible in the UART and can drive GPIO_PPM_IN as an output */ -#define GPIO_PPM_IN_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0) +#define GPIO_PPM_IN_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN7) #define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_PPM_IN_AS_OUT) #define SPEKTRUM_RX_AS_UART() /* Can be left as uart */ #define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true)) @@ -156,7 +172,7 @@ #define BOARD_HAS_STATIC_MANIFEST 1 -#define BOARD_NUM_IO_TIMERS 5 +#define BOARD_NUM_IO_TIMERS 6 #define BOARD_ENABLE_CONSOLE_BUFFER @@ -171,9 +187,12 @@ GPIO_CAN2_SILENT_S0, \ GPIO_nPOWER_IN_A, \ GPIO_VDD_3V3_SPEKTRUM_POWER_EN, \ + GPIO_LEVEL_SHIFTER_OE, \ GPIO_TONE_ALARM_IDLE, \ GPIO_SAFETY_SWITCH_IN, \ GPIO_OTGFS_VBUS, \ + GPIO_BTN_SAFETY, \ + GPIO_LED_SAFETY, \ } __BEGIN_DECLS diff --git a/boards/mro/ctrl-zero-classic/src/i2c.cpp b/boards/mro/ctrl-zero-classic/src/i2c.cpp index 1b8927c69939..49c9ea1c7efd 100644 --- a/boards/mro/ctrl-zero-classic/src/i2c.cpp +++ b/boards/mro/ctrl-zero-classic/src/i2c.cpp @@ -35,6 +35,5 @@ constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { initI2CBusExternal(1), - initI2CBusExternal(3), initI2CBusExternal(4), }; diff --git a/boards/mro/ctrl-zero-classic/src/spi.cpp b/boards/mro/ctrl-zero-classic/src/spi.cpp index 4a4c3502bbd9..674ae3f09b2f 100644 --- a/boards/mro/ctrl-zero-classic/src/spi.cpp +++ b/boards/mro/ctrl-zero-classic/src/spi.cpp @@ -37,7 +37,7 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortC, GPIO::Pin2}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortG, GPIO::Pin3}, SPI::DRDY{GPIO::PortG, GPIO::Pin2}), initSPIDevice(DRV_IMU_DEVTYPE_ICM20948, SPI::CS{GPIO::PortE, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin12}), }, {GPIO::PortE, GPIO::Pin3}), initSPIBus(SPI::Bus::SPI2, { @@ -46,7 +46,10 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { }), initSPIBus(SPI::Bus::SPI5, { initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin10}, SPI::DRDY{GPIO::PortF, GPIO::Pin3}), - initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin6}, SPI::DRDY{GPIO::PortF, GPIO::Pin1}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin0}, SPI::DRDY{GPIO::PortF, GPIO::Pin1}), + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortG, GPIO::Pin9}), }), }; diff --git a/boards/mro/ctrl-zero-classic/src/timer_config.cpp b/boards/mro/ctrl-zero-classic/src/timer_config.cpp index 23bb7a1decc7..50760e387bba 100644 --- a/boards/mro/ctrl-zero-classic/src/timer_config.cpp +++ b/boards/mro/ctrl-zero-classic/src/timer_config.cpp @@ -33,6 +33,28 @@ #include +/* Timer allocation + * + * TIM1_CH4 T FMU_CH1 + * TIM1_CH3 T FMU_CH2 + * TIM1_CH2 T FMU_CH3 + * TIM1_CH1 T FMU_CH4 + * + * TIM4_CH2 T FMU_CH5 + * TIM4_CH3 T FMU_CH6 + * TIM2_CH3 T FMU_CH7 + * TIM2_CH1 T FMU_CH8 + * + * TIM2_CH4 T FMU_CH9 + * TIM15_CH1 T FMU_CH10 + * + * TIM8_CH1 T FMU_CH11 + * + * TIM4_CH4 T FMU_CH12 + * + * TIM16_CH1 T BUZZER - Driven by other driver + */ + constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { initIOTimer(Timer::Timer1, DMA{DMA::Index1}), initIOTimer(Timer::Timer4, DMA{DMA::Index1}), diff --git a/boards/mro/ctrl-zero-h7-oem/nuttx-config/include/board.h b/boards/mro/ctrl-zero-h7-oem/nuttx-config/include/board.h index 9b1ae6342d33..d5ffa2c5848e 100644 --- a/boards/mro/ctrl-zero-h7-oem/nuttx-config/include/board.h +++ b/boards/mro/ctrl-zero-h7-oem/nuttx-config/include/board.h @@ -192,7 +192,7 @@ #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_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 /* ADC 1 2 3 clock source */ #define STM32_RCC_D2CCIP1R_FDCANSEL RCC_D2CCIP1R_FDCANSEL_HSE /* FDCAN 1 2 clock source */ #define STM32_FDCANCLK STM32_HSE_FREQUENCY diff --git a/boards/mro/ctrl-zero-h7/nuttx-config/include/board.h b/boards/mro/ctrl-zero-h7/nuttx-config/include/board.h index 78a0048760b6..cdbd3dd3e629 100644 --- a/boards/mro/ctrl-zero-h7/nuttx-config/include/board.h +++ b/boards/mro/ctrl-zero-h7/nuttx-config/include/board.h @@ -191,7 +191,7 @@ #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_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 /* ADC 1 2 3 clock source */ #define STM32_RCC_D2CCIP1R_FDCANSEL RCC_D2CCIP1R_FDCANSEL_HSE /* FDCAN 1 2 clock source */ #define STM32_FDCANCLK STM32_HSE_FREQUENCY diff --git a/boards/mro/pixracerpro/nuttx-config/include/board.h b/boards/mro/pixracerpro/nuttx-config/include/board.h index 2377d529f46b..5edda931dc10 100644 --- a/boards/mro/pixracerpro/nuttx-config/include/board.h +++ b/boards/mro/pixracerpro/nuttx-config/include/board.h @@ -191,7 +191,7 @@ #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_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 /* ADC 1 2 3 clock source */ #define STM32_RCC_D2CCIP1R_FDCANSEL RCC_D2CCIP1R_FDCANSEL_HSE /* FDCAN 1 2 clock source */ #define STM32_FDCANCLK STM32_HSE_FREQUENCY diff --git a/boards/px4/fmu-v5/rover.px4board b/boards/px4/fmu-v5/rover.px4board new file mode 100644 index 000000000000..3284f0bb7a1b --- /dev/null +++ b/boards/px4/fmu-v5/rover.px4board @@ -0,0 +1,21 @@ +CONFIG_DRIVERS_IRLOCK=n +CONFIG_MODULES_AIRSPEED_SELECTOR=n +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n +CONFIG_MODULES_FLIGHT_MODE_MANAGER=n +CONFIG_MODULES_FW_ATT_CONTROL=n +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n +CONFIG_MODULES_FW_POS_CONTROL=n +CONFIG_MODULES_FW_RATE_CONTROL=n +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n +CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n +CONFIG_MODULES_MC_ATT_CONTROL=n +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n +CONFIG_MODULES_MC_POS_CONTROL=n +CONFIG_MODULES_MC_RATE_CONTROL=n +CONFIG_MODULES_UUV_ATT_CONTROL=n +CONFIG_MODULES_UUV_POS_CONTROL=n +CONFIG_MODULES_VTOL_ATT_CONTROL=n +CONFIG_EKF2_AUX_GLOBAL_POSITION=y +# CONFIG_EKF2_WIND is not set +CONFIG_MODULES_DIFFERENTIAL_DRIVE=y diff --git a/boards/px4/fmu-v5x/default.px4board b/boards/px4/fmu-v5x/default.px4board index 877e2d2e25bb..2bede96763c0 100644 --- a/boards/px4/fmu-v5x/default.px4board +++ b/boards/px4/fmu-v5x/default.px4board @@ -53,7 +53,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y +CONFIG_MODULES_DIFFERENTIAL_DRIVE=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y @@ -81,7 +81,6 @@ CONFIG_MODULES_NAVIGATOR=y CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF=y CONFIG_MODULES_PAYLOAD_DELIVERER=y CONFIG_MODULES_RC_UPDATE=y -CONFIG_MODULES_ROVER_POS_CONTROL=y CONFIG_MODULES_SENSORS=y CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y @@ -102,9 +101,7 @@ CONFIG_SYSTEMCMDS_NSHTERM=y CONFIG_SYSTEMCMDS_PARAM=y CONFIG_SYSTEMCMDS_PERF=y CONFIG_SYSTEMCMDS_REBOOT=y -CONFIG_SYSTEMCMDS_REFLECT=y CONFIG_SYSTEMCMDS_SD_BENCH=y -CONFIG_SYSTEMCMDS_SD_STRESS=y CONFIG_SYSTEMCMDS_SYSTEM_TIME=y CONFIG_SYSTEMCMDS_TOP=y CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y diff --git a/boards/px4/fmu-v5x/init/rc.board_defaults b/boards/px4/fmu-v5x/init/rc.board_defaults index cf152b948eda..30ff624b2f63 100644 --- a/boards/px4/fmu-v5x/init/rc.board_defaults +++ b/boards/px4/fmu-v5x/init/rc.board_defaults @@ -16,7 +16,7 @@ param set-default SENS_EN_INA238 0 param set-default SENS_EN_INA228 0 param set-default SENS_EN_INA226 1 -if ver hwtypecmp V5X009000 V5X009001 V5X00a000 V5X00a001 V5X008000 V5X008001 V5X010001 +if ver hwbasecmp 008 009 00a 010 011 then # Skynode: use the "custom participant" config for uxrce_dds_client param set-default UXRCE_DDS_PTCFG 2 @@ -25,12 +25,12 @@ fi safety_button start # GPIO Expander driver on external I2C3 -if ver hwtypecmp V5X009000 V5X009001 +if ver hwbasecmp 009 then # No USB mcp23009 start -b 3 -X -D 0xf0 -O 0xf0 -P 0x0f -U 10 fi -if ver hwtypecmp V5X00a000 V5X00a001 V5X008000 V5X008001 +if ver hwbasecmp 00a 008 then mcp23009 start -b 3 -X -D 0xf1 -O 0xf0 -P 0x0f -U 10 fi diff --git a/boards/px4/fmu-v5x/init/rc.board_mavlink b/boards/px4/fmu-v5x/init/rc.board_mavlink index ae59f79dffa1..1c85ceff4fe1 100644 --- a/boards/px4/fmu-v5x/init/rc.board_mavlink +++ b/boards/px4/fmu-v5x/init/rc.board_mavlink @@ -3,7 +3,7 @@ # board specific MAVLink startup script. #------------------------------------------------------------------------------ -if ver hwtypecmp V5X009000 V5X009001 V5X00a000 V5X00a001 V5X008000 V5X008001 V5X010001 +if ver hwbasecmp 008 009 00a 010 011 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 diff --git a/boards/px4/fmu-v5x/init/rc.board_sensors b/boards/px4/fmu-v5x/init/rc.board_sensors index d6b91f4505fe..1a0f426db6f4 100644 --- a/boards/px4/fmu-v5x/init/rc.board_sensors +++ b/boards/px4/fmu-v5x/init/rc.board_sensors @@ -5,7 +5,7 @@ set HAVE_PM2 yes -if ver hwtypecmp V5X005000 V5X005001 V5X005002 +if mft query -q -k MFT -s MFT_PM2 -v 0 then set HAVE_PM2 no fi @@ -49,54 +49,53 @@ then fi fi -if ver hwtypecmp V5X000000 V5X000001 V5X000002 V5X001000 V5X004000 V5X004001 V5X004002 V5X005001 V5X005002 +if ver hwbasecmp 008 009 00a 010 011 then - #FMUv5Xbase board orientation + #SKYNODE base fmu board orientation - if ver hwtypecmp V5X000000 V5X000001 V5X004000 V5X004001 V5X005001 + if ver hwtypecmp V5X000 V5X001 then # Internal SPI BMI088 - bmi088 -A -R 4 -s start - bmi088 -G -R 4 -s start + bmi088 -A -R 2 -s start + bmi088 -G -R 2 -s start else # Internal SPI bus ICM20649 - icm20649 -s -R 6 start + icm20649 -s -R 4 start fi # Internal SPI bus ICM42688p - icm42688p -R 6 -s start + icm42688p -R 4 -s start # Internal SPI bus ICM-20602 (hard-mounted) - icm20602 -R 10 -s start + icm20602 -R 8 -s start # Internal magnetometer on I2c - bmm150 -I start + bmm150 -I -R 6 start + + # Auto start power monitors + pm_selector_auterion start else - #SKYNODE base fmu board orientation + #FMUv5Xbase board orientation - if ver hwtypecmp V5X009000 V5X009001 V5X00a000 V5X00a001 V5X008000 V5X008001 V5X010001 + if ver hwtypecmp V5X000 V5X001 then # Internal SPI BMI088 - bmi088 -A -R 2 -s start - bmi088 -G -R 2 -s start + bmi088 -A -R 4 -s start + bmi088 -G -R 4 -s start else # Internal SPI bus ICM20649 - icm20649 -s -R 4 start + icm20649 -s -R 6 start fi # Internal SPI bus ICM42688p - icm42688p -R 4 -s start + icm42688p -R 6 -s start # Internal SPI bus ICM-20602 (hard-mounted) - icm20602 -R 8 -s start + icm20602 -R 10 -s start # Internal magnetometer on I2c - bmm150 -I -R 6 start - - # Auto start power monitors - pm_selector_auterion start - + bmm150 -I 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) @@ -108,7 +107,7 @@ ist8310 -X -b 1 -R 10 start if param compare SENS_INT_BARO_EN 1 then bmp388 -I -a 0x77 start - if ver hwtypecmp V5X000000 V5X001000 V5X008000 V5X009000 V5X00a000 + if ver hwtypecmp V5X000 then bmp388 -I start else diff --git a/boards/px4/fmu-v5x/rover.px4board b/boards/px4/fmu-v5x/rover.px4board new file mode 100644 index 000000000000..b8a1597b30ea --- /dev/null +++ b/boards/px4/fmu-v5x/rover.px4board @@ -0,0 +1,16 @@ +CONFIG_DRIVERS_IRLOCK=n +CONFIG_MODULES_AIRSPEED_SELECTOR=n +CONFIG_MODULES_FLIGHT_MODE_MANAGER=n +CONFIG_MODULES_FW_ATT_CONTROL=n +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n +CONFIG_MODULES_FW_POS_CONTROL=n +CONFIG_MODULES_FW_RATE_CONTROL=n +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n +CONFIG_MODULES_MC_ATT_CONTROL=n +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n +CONFIG_MODULES_MC_POS_CONTROL=n +CONFIG_MODULES_MC_RATE_CONTROL=n +CONFIG_MODULES_VTOL_ATT_CONTROL=n +CONFIG_EKF2_AUX_GLOBAL_POSITION=y +# CONFIG_EKF2_WIND is not set diff --git a/boards/px4/fmu-v5x/src/CMakeLists.txt b/boards/px4/fmu-v5x/src/CMakeLists.txt index 684b11736eda..311b7c9be8c9 100644 --- a/boards/px4/fmu-v5x/src/CMakeLists.txt +++ b/boards/px4/fmu-v5x/src/CMakeLists.txt @@ -36,7 +36,6 @@ add_library(drivers_board i2c.cpp init.cpp led.c - manifest.c mtd.cpp sdio.c spi.cpp diff --git a/boards/px4/fmu-v5x/src/board_config.h b/boards/px4/fmu-v5x/src/board_config.h index 67cac1acd35f..2fe1654b65f5 100644 --- a/boards/px4/fmu-v5x/src/board_config.h +++ b/boards/px4/fmu-v5x/src/board_config.h @@ -179,31 +179,17 @@ /* HW Version and Revision drive signals Default to 1 to detect */ -#define BOARD_HAS_HW_VERSIONING +#define BOARD_HAS_HW_SPLIT_VERSIONING #define GPIO_HW_VER_REV_DRIVE /* PG0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN0) #define GPIO_HW_REV_SENSE /* PF5 */ ADC3_GPIO(15) #define GPIO_HW_VER_SENSE /* PF4 */ ADC3_GPIO(14) #define HW_INFO_INIT_PREFIX "V5X" -#define BOARD_NUM_SPI_CFG_HW_VERSIONS 7 -// Base FMUM -#define V5X00 HW_VER_REV(0x0,0x0) // FMUV5X, Rev 0 -#define V5X10 HW_VER_REV(0x1,0x0) // NO PX4IO, Rev 0 -#define V5X01 HW_VER_REV(0x0,0x1) // FMUV5X I2C2 BMP388, Rev 1 -#define V5X02 HW_VER_REV(0x0,0x2) // FMUV5X, Rev 2 -#define V5X40 HW_VER_REV(0x4,0x0) // FMUV5X, HB CM4 base Rev 0 -#define V5X41 HW_VER_REV(0x4,0x1) // FMUV5X I2C2 BMP388, HB CM4 base Rev 1 -#define V5X42 HW_VER_REV(0x4,0x2) // FMUV5X, HB CM4 base Rev 2 -#define V5X50 HW_VER_REV(0x5,0x0) // FMUV5X, HB Mini Rev 0 -#define V5X51 HW_VER_REV(0x5,0x1) // FMUV5X I2C2 BMP388, HB Mini Rev 1 -#define V5X52 HW_VER_REV(0x5,0x2) // FMUV5X, HB Mini Rev 2 -#define V5X90 HW_VER_REV(0x9,0x0) // NO USB, Rev 0 -#define V5X91 HW_VER_REV(0x9,0x1) // NO USB I2C2 BMP388, Rev 1 -#define V5X92 HW_VER_REV(0x9,0x2) // NO USB I2C2 BMP388, Rev 2 -#define V5Xa0 HW_VER_REV(0xa,0x0) // NO USB (Q), Rev 0 -#define V5Xa1 HW_VER_REV(0xa,0x1) // NO USB (Q) I2C2 BMP388, Rev 1 -#define V5Xa2 HW_VER_REV(0xa,0x2) // NO USB (Q) I2C2 BMP388, Rev 2 -#define V5X101 HW_VER_REV(0x10,0x1) // NO USB (Q) I2C2 BMP388, Rev 1 +#define BOARD_NUM_SPI_CFG_HW_VERSIONS 3 + +#define V5X_0 HW_FMUM_ID(0x0) // FMUV5X, Auterion FMUv5x RC13 (baro2 BMP388 on I2C4) Sensor Set Rev 0 +#define V5X_1 HW_FMUM_ID(0x1) // FMUV5X, Auterion, HB FMUv5x RC15 (baro2 BMP388 on I2C2) Sensor Set Rev 1 +#define V5X_2 HW_FMUM_ID(0x2) // FMUV5X, HB FMUv5x Sensor Set Rev 2 #define UAVCAN_NUM_IFACES_RUNTIME 1 diff --git a/boards/px4/fmu-v5x/src/manifest.c b/boards/px4/fmu-v5x/src/manifest.c deleted file mode 100644 index 56f0009f5000..000000000000 --- a/boards/px4/fmu-v5x/src/manifest.c +++ /dev/null @@ -1,222 +0,0 @@ -/**************************************************************************** - * - * 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_v0500[] = { - { - // PX4_MFT_PX4IO - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, - { - // PX4_MFT_USB - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, - { - // PX4_MFT_CAN2 - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, -}; - -static const px4_hw_mft_item_t hw_mft_list_v0550[] = { - { - // PX4_MFT_PX4IO - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, - { - // PX4_MFT_USB - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, - { - // PX4_MFT_CAN2 - .present = 0, - .mandatory = 0, - .connection = px4_hw_con_unknown, - }, -}; - -static const px4_hw_mft_item_t hw_mft_list_v0510[] = { - { - // PX4_MFT_PX4IO - .present = 0, - .mandatory = 0, - .connection = px4_hw_con_unknown, - }, - { - // PX4_MFT_USB - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, - { - // PX4_MFT_CAN2 - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, -}; - -static const px4_hw_mft_item_t hw_mft_list_v0509[] = { - { - // PX4_MFT_PX4IO - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, - { - // PX4_MFT_USB - .present = 0, - .mandatory = 0, - .connection = px4_hw_con_unknown, - }, - { - // PX4_MFT_CAN2 - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, -}; - -static px4_hw_mft_list_entry_t mft_lists[] = { -// ver_rev - {V5X00, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // FMUV5X, Rev 0 - {V5X01, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // FMUV5X, Rev 1 - {V5X02, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // FMUV5X, Rev 2 - {V5X10, hw_mft_list_v0510, arraySize(hw_mft_list_v0510)}, // NO PX4IO, Rev 0 - {V5X41, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // FMUV5X, HB CM4 base Rev 1 - {V5X42, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // FMUV5X, HB CM4 base Rev 2 - {V5X51, hw_mft_list_v0550, arraySize(hw_mft_list_v0550)}, // FMUV5X, HB Mini Rev 1 - {V5X52, hw_mft_list_v0550, arraySize(hw_mft_list_v0550)}, // FMUV5X, HB Mini Rev 2 - {V5X90, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB, Rev 0 - {V5X91, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB I2C2 BMP388, Rev 1 - {V5X92, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB I2C2 BMP388, Rev 2 - {V5Xa0, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB (Q), Rev 0 - {V5Xa1, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB (Q) I2C2 BMP388, Rev 1 - {V5Xa2, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB (Q) I2C2 BMP388, Rev 2 - {V5X101, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB I2C2 BMP388, Rev 1 -}; - -/************************************************************************************ - * 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/px4/fmu-v5x/src/mtd.cpp b/boards/px4/fmu-v5x/src/mtd.cpp index c0139d123279..8e57555eacad 100644 --- a/boards/px4/fmu-v5x/src/mtd.cpp +++ b/boards/px4/fmu-v5x/src/mtd.cpp @@ -31,6 +31,9 @@ * ****************************************************************************/ +#include +#include + #include #include // KiB BS nB @@ -114,10 +117,16 @@ static const px4_mft_entry_s mtd_mft = { .pmft = (void *) &board_mtd_config, }; +static const px4_mft_entry_s mft_mft = { + .type = MFT, + .pmft = (void *) system_query_manifest, +}; + static const px4_mft_s mft = { - .nmft = 1, + .nmft = 2, .mfts = { - &mtd_mft + &mtd_mft, + &mft_mft, } }; diff --git a/boards/px4/fmu-v5x/src/spi.cpp b/boards/px4/fmu-v5x/src/spi.cpp index 8fa378ee5d4d..dd6bfbb25e1e 100644 --- a/boards/px4/fmu-v5x/src/spi.cpp +++ b/boards/px4/fmu-v5x/src/spi.cpp @@ -36,7 +36,7 @@ #include constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSIONS] = { - initSPIHWVersion(V5X00, { + initSPIFmumID(V5X_0, { initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), }, {GPIO::PortI, GPIO::Pin11}), @@ -60,7 +60,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), - initSPIHWVersion(V5X01, { + initSPIFmumID(V5X_1, { initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), }, {GPIO::PortI, GPIO::Pin11}), @@ -84,7 +84,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), - initSPIHWVersion(V5X02, { + initSPIFmumID(V5X_2, { initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), }, {GPIO::PortI, GPIO::Pin11}), @@ -107,99 +107,6 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), - initSPIHWVersion(V5X41, { - initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, 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::PortH, GPIO::Pin12}), - }, {GPIO::PortD, GPIO::Pin15}), - 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}), - }, {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(V5X42, { - initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, 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::PortH, GPIO::Pin12}), - }, {GPIO::PortD, GPIO::Pin15}), - 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(V5X51, { - initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, 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::PortH, GPIO::Pin12}), - }, {GPIO::PortD, GPIO::Pin15}), - 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}), - }, {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(V5X52, { - initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, 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::PortH, GPIO::Pin12}), - }, {GPIO::PortD, GPIO::Pin15}), - 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}), - }), - }), }; static constexpr bool unused = validateSPIConfig(px4_spi_buses_all_hw); diff --git a/boards/px4/fmu-v6c/nuttx-config/include/board.h b/boards/px4/fmu-v6c/nuttx-config/include/board.h index 8042d466a7bb..1951031cb825 100644 --- a/boards/px4/fmu-v6c/nuttx-config/include/board.h +++ b/boards/px4/fmu-v6c/nuttx-config/include/board.h @@ -248,7 +248,7 @@ /* ADC 1 2 3 clock source */ -#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2 +#define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 /* FDCAN 1 2 clock source */ diff --git a/boards/px4/fmu-v6c/rover.px4board b/boards/px4/fmu-v6c/rover.px4board new file mode 100644 index 000000000000..f754c54e36f7 --- /dev/null +++ b/boards/px4/fmu-v6c/rover.px4board @@ -0,0 +1,16 @@ +CONFIG_MODULES_AIRSPEED_SELECTOR=n +CONFIG_MODULES_FLIGHT_MODE_MANAGER=n +CONFIG_MODULES_FW_ATT_CONTROL=n +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n +CONFIG_MODULES_FW_POS_CONTROL=n +CONFIG_MODULES_FW_RATE_CONTROL=n +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n +CONFIG_MODULES_MC_ATT_CONTROL=n +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n +CONFIG_MODULES_MC_POS_CONTROL=n +CONFIG_MODULES_MC_RATE_CONTROL=n +CONFIG_MODULES_VTOL_ATT_CONTROL=n +CONFIG_EKF2_AUX_GLOBAL_POSITION=y +# CONFIG_EKF2_WIND is not set +CONFIG_MODULES_DIFFERENTIAL_DRIVE=y diff --git a/boards/px4/fmu-v6u/nuttx-config/include/board.h b/boards/px4/fmu-v6u/nuttx-config/include/board.h index a82a7bf098a8..826c5df337b4 100644 --- a/boards/px4/fmu-v6u/nuttx-config/include/board.h +++ b/boards/px4/fmu-v6u/nuttx-config/include/board.h @@ -248,7 +248,7 @@ /* ADC 1 2 3 clock source */ -#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2 +#define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 /* CAN FD clock source */ #define STM32_FDCANCLK STM32_HSE_FREQUENCY diff --git a/boards/px4/fmu-v6u/rover.px4board b/boards/px4/fmu-v6u/rover.px4board new file mode 100644 index 000000000000..f754c54e36f7 --- /dev/null +++ b/boards/px4/fmu-v6u/rover.px4board @@ -0,0 +1,16 @@ +CONFIG_MODULES_AIRSPEED_SELECTOR=n +CONFIG_MODULES_FLIGHT_MODE_MANAGER=n +CONFIG_MODULES_FW_ATT_CONTROL=n +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n +CONFIG_MODULES_FW_POS_CONTROL=n +CONFIG_MODULES_FW_RATE_CONTROL=n +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n +CONFIG_MODULES_MC_ATT_CONTROL=n +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n +CONFIG_MODULES_MC_POS_CONTROL=n +CONFIG_MODULES_MC_RATE_CONTROL=n +CONFIG_MODULES_VTOL_ATT_CONTROL=n +CONFIG_EKF2_AUX_GLOBAL_POSITION=y +# CONFIG_EKF2_WIND is not set +CONFIG_MODULES_DIFFERENTIAL_DRIVE=y diff --git a/boards/px4/fmu-v6x/init/rc.board_defaults b/boards/px4/fmu-v6x/init/rc.board_defaults index 93acc0fbd294..95d873475b15 100644 --- a/boards/px4/fmu-v6x/init/rc.board_defaults +++ b/boards/px4/fmu-v6x/init/rc.board_defaults @@ -18,7 +18,7 @@ param set-default SENS_EN_INA238 0 param set-default SENS_EN_INA228 0 param set-default SENS_EN_INA226 0 -if ver hwtypecmp V6X009010 V6X010010 +if ver hwbasecmp 009 010 011 then # Skynode: use the "custom participant" config for uxrce_dds_client param set-default UXRCE_DDS_PTCFG 2 diff --git a/boards/px4/fmu-v6x/init/rc.board_mavlink b/boards/px4/fmu-v6x/init/rc.board_mavlink index 6001553a76f3..713d7a41b72b 100644 --- a/boards/px4/fmu-v6x/init/rc.board_mavlink +++ b/boards/px4/fmu-v6x/init/rc.board_mavlink @@ -4,7 +4,7 @@ #------------------------------------------------------------------------------ # if skynode base board is detected start Mavlink on Telem2 -if ver hwtypecmp V6X009010 V6X010010 +if ver hwbasecmp 009 010 011 then mavlink start -d /dev/ttyS4 -b 3000000 -r 290000 -m onboard_low_bandwidth -x -z diff --git a/boards/px4/fmu-v6x/init/rc.board_sensors b/boards/px4/fmu-v6x/init/rc.board_sensors index 6a47d72f0e77..f7b132af1117 100644 --- a/boards/px4/fmu-v6x/init/rc.board_sensors +++ b/boards/px4/fmu-v6x/init/rc.board_sensors @@ -5,13 +5,14 @@ set HAVE_PM2 yes set INA_CONFIGURED no -if ver hwtypecmp V6X005000 V6X005001 V6X005003 V6X005004 +if mft query -q -k MFT -s MFT_PM2 -v 0 then set HAVE_PM2 no fi if param compare -s ADC_ADS1115_EN 1 then ads1115 start -X + board_adc start -n else board_adc start fi @@ -55,7 +56,7 @@ then fi #Start Auterion Power Module selector for Skynode boards -if ver hwtypecmp V6X009010 V6X010010 +if ver hwbasecmp 009 010 011 then pm_selector_auterion start else @@ -70,25 +71,34 @@ else fi fi -if ver hwtypecmp V6X000006 V6X004006 V6X005006 +# Keep nesting shallow +if ver hwtypecmp V6X006 V6X008 then - # Internal SPI bus ICM45686 - icm45686 -s -R 10 start - iim42652 -s -R 6 start - adis16470 -s -R 0 start + if ver hwtypecmp V6X006 + then + # Internal SPI bus ICM45686 + adis16470 -s -R 0 start + iim42652 -s -R 6 start + icm45686 -s -R 10 start + else + # Internal SPI bus 3x ICM45686 + icm45686 -b 3 -s -R 0 start + icm45686 -b 2 -s -R 0 start + icm45686 -b 1 -s -R 10 start + fi else - if ver hwtypecmp V6X000004 V6X001004 V6X004004 V6X005004 + if ver hwtypecmp V6X004 then # Internal SPI bus ICM20649 icm20649 -s -R 6 start else # Internal SPI BMI088 - if ver hwtypecmp V6X009010 V6X010010 + if ver hwbasecmp 009 010 011 then bmi088 -A -R 6 -s start bmi088 -G -R 6 -s start else - if ver hwtypecmp V6X000010 + if ver hwtypecmp V6X010 then bmi088 -A -R 0 -s start bmi088 -G -R 0 -s start @@ -100,11 +110,11 @@ else fi # Internal SPI bus ICM42688p - if ver hwtypecmp V6X009010 V6X010010 + if ver hwbasecmp 009 010 011 then icm42688p -R 12 -s start else - if ver hwtypecmp V6X000010 + if ver hwtypecmp V6X010 then icm42688p -R 14 -s start else @@ -112,12 +122,12 @@ else fi fi - if ver hwtypecmp V6X000003 V6X001003 V6X003003 V6X000004 V6X001004 V6X004003 V6X004004 V6X005003 V6X005004 + if ver hwtypecmp V6X003 V6X004 then # Internal SPI bus ICM-42670-P (hard-mounted) icm42670p -R 10 -s start else - if ver hwtypecmp V6X009010 V6X010010 + if ver hwbasecmp 009 010 011 then icm20602 -R 6 -s start else @@ -128,7 +138,7 @@ else fi # Internal magnetometer on I2c -if ver hwtypecmp V6X002001 +if ver hwtypecmp V6X001 then rm3100 -I -b 4 start else @@ -142,7 +152,7 @@ ist8310 -X -b 1 -R 10 start # Possible internal Baro if param compare SENS_INT_BARO_EN 1 then - if ver hwtypecmp V6X002001 V6X000006 V6X004006 V6X005006 + if ver hwtypecmp V6X001 V6X006 V6X008 then icp201xx -I -a 0x64 start else @@ -151,7 +161,7 @@ then fi #external baro -if ver hwtypecmp V6X002001 +if ver hwtypecmp V6X001 then icp201xx -X start else diff --git a/boards/px4/fmu-v6x/nuttx-config/include/board.h b/boards/px4/fmu-v6x/nuttx-config/include/board.h index eb3004183b02..d6b2925b795c 100644 --- a/boards/px4/fmu-v6x/nuttx-config/include/board.h +++ b/boards/px4/fmu-v6x/nuttx-config/include/board.h @@ -248,7 +248,7 @@ /* ADC 1 2 3 clock source */ -#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2 +#define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 /* FDCAN 1 2 clock source */ @@ -380,7 +380,9 @@ #define GPIO_UART5_RX GPIO_UART5_RX_3 /* PD2 */ #define GPIO_UART5_TX GPIO_UART5_TX_3 /* PC12 */ // GPIO_UART5_RTS no remap /* PC8 */ -// GPIO_UART5_CTS No remap /* PC9 */ +#undef GPIO_UART5_CTS +#define GPIO_UART5_CTS ((GPIO_ALT|GPIO_AF8|GPIO_PORTC|GPIO_PIN9) | GPIO_PULLDOWN) /* PC9 */ + #define GPIO_USART6_RX GPIO_USART6_RX_1 /* PC7 */ #define GPIO_USART6_TX GPIO_USART6_TX_1 /* PC6 */ diff --git a/boards/px4/fmu-v6x/rover.px4board b/boards/px4/fmu-v6x/rover.px4board new file mode 100644 index 000000000000..f754c54e36f7 --- /dev/null +++ b/boards/px4/fmu-v6x/rover.px4board @@ -0,0 +1,16 @@ +CONFIG_MODULES_AIRSPEED_SELECTOR=n +CONFIG_MODULES_FLIGHT_MODE_MANAGER=n +CONFIG_MODULES_FW_ATT_CONTROL=n +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n +CONFIG_MODULES_FW_POS_CONTROL=n +CONFIG_MODULES_FW_RATE_CONTROL=n +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n +CONFIG_MODULES_MC_ATT_CONTROL=n +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n +CONFIG_MODULES_MC_POS_CONTROL=n +CONFIG_MODULES_MC_RATE_CONTROL=n +CONFIG_MODULES_VTOL_ATT_CONTROL=n +CONFIG_EKF2_AUX_GLOBAL_POSITION=y +# CONFIG_EKF2_WIND is not set +CONFIG_MODULES_DIFFERENTIAL_DRIVE=y diff --git a/boards/px4/fmu-v6x/src/CMakeLists.txt b/boards/px4/fmu-v6x/src/CMakeLists.txt index d4a16ff4f303..a120ebe33623 100644 --- a/boards/px4/fmu-v6x/src/CMakeLists.txt +++ b/boards/px4/fmu-v6x/src/CMakeLists.txt @@ -55,7 +55,6 @@ else() init.c led.c mtd.cpp - manifest.c sdio.c spi.cpp timer_config.cpp diff --git a/boards/px4/fmu-v6x/src/board_config.h b/boards/px4/fmu-v6x/src/board_config.h index ff0676fab7ee..7cad5497ed2b 100644 --- a/boards/px4/fmu-v6x/src/board_config.h +++ b/boards/px4/fmu-v6x/src/board_config.h @@ -208,40 +208,22 @@ #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 BOARD_HAS_HW_SPLIT_VERSIONING #define GPIO_HW_VER_REV_DRIVE /* PG0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN0) #define GPIO_HW_REV_SENSE /* PH4 */ GPIO_ADC3_INP15 #define GPIO_HW_VER_SENSE /* PH3 */ GPIO_ADC3_INP14 #define HW_INFO_INIT_PREFIX "V6X" -#define BOARD_NUM_SPI_CFG_HW_VERSIONS 13 // Rev 0 and Rev 3,4,6 Sensor sets +#define BOARD_NUM_SPI_CFG_HW_VERSIONS 7 // 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 - - +#define V6X_0 HW_FMUM_ID(0x0) // FMUV6X, Auterion,HB Sensor Set Rev 0 +#define V6X_1 HW_FMUM_ID(0x1) // FMUV6X, CUAV Sensor Set Rev 1 +#define V6X_3 HW_FMUM_ID(0x3) // FMUV6X, HB Sensor Set Rev 3 +#define V6X_4 HW_FMUM_ID(0x4) // FMUV6X, HB Sensor Set Rev 4 +#define V6X_6 HW_FMUM_ID(0x6) // FMUV6X, HB Sensor Set Rev 6 +#define V6X_8 HW_FMUM_ID(0x8) // FMUV6X, HB Sensor Set Rev 8 +#define V6X_16 HW_FMUM_ID(0x10) // FMUV6X, Auterion Sensor Set Rev 16 from EEPROM #define UAVCAN_NUM_IFACES_RUNTIME 1 diff --git a/boards/px4/fmu-v6x/src/manifest.c b/boards/px4/fmu-v6x/src/manifest.c deleted file mode 100644 index bda800b74919..000000000000 --- a/boards/px4/fmu-v6x/src/manifest.c +++ /dev/null @@ -1,208 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2018-2021 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file manifest.c - * - * This module supplies the interface to the manifest of hardware that is - * optional and dependent on the HW REV and HW VER IDs - * - * The manifest allows the system to know whether a hardware option - * say for example the PX4IO is an no-pop option vs it is broken. - * - */ - -/**************************************************************************** - * Included Files - ****************************************************************************/ - -#include -#include - -#include -#include -#include - -#include "systemlib/px4_macros.h" - -/**************************************************************************** - * Pre-Processor Definitions - ****************************************************************************/ - -typedef struct { - uint32_t hw_ver_rev; /* the version and revision */ - const px4_hw_mft_item_t *mft; /* The first entry */ - uint32_t entries; /* the lenght of the list */ -} px4_hw_mft_list_entry_t; - -typedef px4_hw_mft_list_entry_t *px4_hw_mft_list_entry; -#define px4_hw_mft_list_uninitialized (px4_hw_mft_list_entry) -1 - -static const px4_hw_mft_item_t device_unsupported = {0, 0, 0}; - -// List of components on a specific board configuration -// The index of those components is given by the enum (px4_hw_mft_item_id_t) -// declared in board_common.h -static const px4_hw_mft_item_t hw_mft_list_v0600[] = { - { - // PX4_MFT_PX4IO - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, - { - // PX4_MFT_USB - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, - { - // PX4_MFT_CAN2 - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, -}; - -static const px4_hw_mft_item_t hw_mft_list_v0610[] = { - { - // PX4_MFT_PX4IO - .present = 0, - .mandatory = 0, - .connection = px4_hw_con_unknown, - }, - { - // PX4_MFT_USB - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, - { - // PX4_MFT_CAN2 - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, -}; - -static const px4_hw_mft_item_t hw_mft_list_v0650[] = { - { - // PX4_MFT_PX4IO - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_unknown, - }, - { - // PX4_MFT_USB - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, - { - // PX4_MFT_CAN2 - .present = 0, - .mandatory = 0, - .connection = px4_hw_con_unknown, - }, -}; - - -static px4_hw_mft_list_entry_t mft_lists[] = { -// ver_rev - {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 - {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 - {V6X1010, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // FMUV6X, rev from EEPROM Auterion Skynode ver10 -}; - -/************************************************************************************ - * 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/px4/fmu-v6x/src/mtd.cpp b/boards/px4/fmu-v6x/src/mtd.cpp index c0139d123279..8e57555eacad 100644 --- a/boards/px4/fmu-v6x/src/mtd.cpp +++ b/boards/px4/fmu-v6x/src/mtd.cpp @@ -31,6 +31,9 @@ * ****************************************************************************/ +#include +#include + #include #include // KiB BS nB @@ -114,10 +117,16 @@ static const px4_mft_entry_s mtd_mft = { .pmft = (void *) &board_mtd_config, }; +static const px4_mft_entry_s mft_mft = { + .type = MFT, + .pmft = (void *) system_query_manifest, +}; + static const px4_mft_s mft = { - .nmft = 1, + .nmft = 2, .mfts = { - &mtd_mft + &mtd_mft, + &mft_mft, } }; diff --git a/boards/px4/fmu-v6x/src/spi.cpp b/boards/px4/fmu-v6x/src/spi.cpp index 8b002e157efa..8c633dd78f14 100644 --- a/boards/px4/fmu-v6x/src/spi.cpp +++ b/boards/px4/fmu-v6x/src/spi.cpp @@ -36,7 +36,7 @@ #include constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSIONS] = { - initSPIHWVersion(V6X00, { + initSPIFmumID(V6X_0, { 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}), @@ -60,54 +60,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), - initSPIHWVersion(V6X03, { - 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_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(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, { + initSPIFmumID(V6X_1, { 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}), @@ -131,7 +84,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), - initSPIHWVersion(V6X43, { + initSPIFmumID(V6X_3, { 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}), @@ -155,7 +108,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), - initSPIHWVersion(V6X44, { + initSPIFmumID(V6X_4, { 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}), @@ -178,7 +131,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), - initSPIHWVersion(V6X46, { + initSPIFmumID(V6X_6, { 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}), @@ -201,86 +154,15 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), - 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(V6X53, { - 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_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(V6X54, { - 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(V6X56, { + initSPIFmumID(V6X_8, { 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}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM45686, 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}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM45686, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), }, {GPIO::PortE, GPIO::Pin7}), // initSPIBus(SPI::Bus::SPI4, { // // no devices @@ -295,31 +177,8 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), - initSPIHWVersion(V6X0910, { - initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, 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}), - }, {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(V6X1010, { + initSPIFmumID(V6X_16, { initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), }, {GPIO::PortI, GPIO::Pin11}), @@ -342,6 +201,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}), }), }), + }; static constexpr bool unused = validateSPIConfig(px4_spi_buses_all_hw); diff --git a/boards/px4/fmu-v6xrt/default.px4board b/boards/px4/fmu-v6xrt/default.px4board index b5e2e23af38c..f9961fd7bbfb 100644 --- a/boards/px4/fmu-v6xrt/default.px4board +++ b/boards/px4/fmu-v6xrt/default.px4board @@ -6,21 +6,30 @@ CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS3" CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS2" CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS5" CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS6" +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_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y -CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_I2C=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y CONFIG_DRIVERS_GPS=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_DRIVERS_MAGNETOMETER_BOSCH_BMM150=y -CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y -CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_DRIVERS_OSD_MSP_OSD=y CONFIG_DRIVERS_POWER_MONITOR_INA226=y CONFIG_DRIVERS_POWER_MONITOR_INA228=y CONFIG_DRIVERS_POWER_MONITOR_INA238=y @@ -32,14 +41,20 @@ CONFIG_COMMON_TELEMETRY=y CONFIG_DRIVERS_TONE_ALARM=y CONFIG_DRIVERS_UAVCAN=y CONFIG_COMMON_UWB=y +CONFIG_MODULES_AIRSPEED_SELECTOR=y CONFIG_MODULES_BATTERY_STATUS=y CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y +CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y CONFIG_MODULES_GYRO_FFT=y @@ -57,11 +72,15 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y CONFIG_MODULES_SENSORS=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_UXRCE_DDS_CLIENT=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_BSONDUMP=y CONFIG_SYSTEMCMDS_DMESG=y CONFIG_SYSTEMCMDS_DUMPFILE=y CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y diff --git a/boards/px4/fmu-v6xrt/firmware.prototype b/boards/px4/fmu-v6xrt/firmware.prototype index beccbcebbd04..a94d419a3d7a 100644 --- a/boards/px4/fmu-v6xrt/firmware.prototype +++ b/boards/px4/fmu-v6xrt/firmware.prototype @@ -7,7 +7,7 @@ "summary": "PX4FMUv6XRT", "version": "0.1", "image_size": 0, - "image_maxsize": 7340032, + "image_maxsize": 4063232, "git_identity": "", "board_revision": 0 } diff --git a/boards/px4/fmu-v6xrt/init/rc.board_defaults b/boards/px4/fmu-v6xrt/init/rc.board_defaults index b242eebc904c..45282796efde 100644 --- a/boards/px4/fmu-v6xrt/init/rc.board_defaults +++ b/boards/px4/fmu-v6xrt/init/rc.board_defaults @@ -16,8 +16,6 @@ param set-default SENS_EN_INA238 0 param set-default SENS_EN_INA228 0 param set-default SENS_EN_INA226 1 -param set-default SYS_USE_IO 1 - safety_button start if param greater -s UAVCAN_ENABLE 0 diff --git a/boards/px4/fmu-v6xrt/init/rc.board_mavlink b/boards/px4/fmu-v6xrt/init/rc.board_mavlink index 633dc58eb813..5c1a00ff959d 100644 --- a/boards/px4/fmu-v6xrt/init/rc.board_mavlink +++ b/boards/px4/fmu-v6xrt/init/rc.board_mavlink @@ -4,7 +4,7 @@ #------------------------------------------------------------------------------ # if skynode base board is detected start Mavlink on Telem2 -if ver hwtypecmp V6XRT009000 V6XRT010000 +if ver hwbasecmp 009 010 011 then mavlink start -d /dev/ttyS5 -b 3000000 -r 290000 -m onboard_low_bandwidth -x -z diff --git a/boards/px4/fmu-v6xrt/init/rc.board_sensors b/boards/px4/fmu-v6xrt/init/rc.board_sensors index 0133d2c60267..64779648a4b2 100644 --- a/boards/px4/fmu-v6xrt/init/rc.board_sensors +++ b/boards/px4/fmu-v6xrt/init/rc.board_sensors @@ -1,6 +1,6 @@ #!/bin/sh # -# PX4 FMUv5 specific board sensors init +# PX4 FMUv6xrt specific board sensors init #------------------------------------------------------------------------------ # # UART mapping on PX4 FMU-V6XRT: @@ -8,17 +8,17 @@ # LPUART1 /dev/ttyS0 CONSOLE # LPUART3 /dev/ttyS1 GPS # LPUART4 /dev/ttyS2 TELEM1 -# LPUART5 /dev/ttyS4 GPS2 -# LPUART6 /dev/ttyS5 PX4IO -# LPUART8 /dev/ttyS6 TELEM2 -# LPUART10 /dev/ttyS7 TELEM3 -# LPUART11 /dev/ttyS8 EXT2 +# LPUART5 /dev/ttyS3 GPS2 +# LPUART6 /dev/ttyS4 PX4IO +# LPUART8 /dev/ttyS5 TELEM2 +# LPUART10 /dev/ttyS6 TELEM3 +# LPUART11 /dev/ttyS7 EXT2 # #------------------------------------------------------------------------------ set HAVE_PM2 yes -if ver hwtypecmp V5X005000 V5X005001 V5X005002 +if mft query -q -k MFT -s MFT_PM2 -v 0 then set HAVE_PM2 no fi @@ -84,6 +84,8 @@ ist8310 -X -b 1 -R 10 start if param compare SENS_INT_BARO_EN 1 then bmp388 -I -b 3 -a 0x77 start - bmp388 -X -b 2 start fi + +bmp388 -X -b 2 start + unset HAVE_PM2 diff --git a/boards/px4/fmu-v6xrt/nuttx-config/include/board.h b/boards/px4/fmu-v6xrt/nuttx-config/include/board.h index 31f0f35a15d6..6b704139dc0c 100644 --- a/boards/px4/fmu-v6xrt/nuttx-config/include/board.h +++ b/boards/px4/fmu-v6xrt/nuttx-config/include/board.h @@ -267,6 +267,7 @@ // This is the ENET_1G interface. +// Compile time selection #if defined(CONFIG_ETH0_PHY_TJA1103) # define BOARD_PHY_ADDR (18) #endif @@ -274,6 +275,35 @@ # define BOARD_PHY_ADDR (0) #endif +/* Run time selection see mii.h */ + +#define BOARD_ETH0_PHY_LIST \ + { \ + "LAN8742A", \ + MII_PHYID1_LAN8742A, \ + MII_PHYID2_LAN8742A, \ + MII_LAN8740_SCSR, \ + 0, \ + 0xffff, \ + MII_LAN8720_SPSCR_10MBPS, \ + MII_LAN8720_SPSCR_100MBPS, \ + MII_LAN8720_SPSCR_DUPLEX, \ + 22, \ + }, \ + { \ + "TJA1103", \ + MII_PHYID1_TJA1103, \ + MII_PHYID2_TJA1103, \ + 0xffff, \ + 18, \ + 0xffff, \ + 0, \ + MII_LAN8720_SPSCR_100MBPS, \ + MII_LAN8720_SPSCR_DUPLEX, \ + 45, \ + }, \ + + #define GPIO_ENET2_TX_DATA00 (GPIO_ENET_1G_TX_DATA0_1|IOMUX_ENET_DATA_DEFAULT) /* GPIO_DISP_B1_09 */ #define GPIO_ENET2_TX_DATA01 (GPIO_ENET_1G_TX_DATA1_1|IOMUX_ENET_DATA_DEFAULT) /* GPIO_DISP_B1_08 */ #define GPIO_ENET2_RX_DATA00 (GPIO_ENET_1G_RX_DATA0_2|IOMUX_ENET_DATA_DEFAULT) /* GPIO_EMC_B2_15 */ @@ -292,16 +322,20 @@ #include #include // add -I build/px4_fmu-v6xrt_default/NuttX/nuttx/arch/arm/src/chip \ to NuttX Makedefs.in -#define PROBE_IOMUX (IOMUX_SPEED_MAX | IOMUX_SLEW_FAST | IOMUX_DRIVE_33OHM | IOMUX_CMOS_OUTPUT | IOMUX_PULL_NONE) +#define PROBE_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_SLEW_FAST) # define PROBE_N(n) (1<<((n)-1)) -# define PROBE_1 /* GPIO_B0_06 */ (GPIO_PORT2 | GPIO_PIN6 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | PROBE_IOMUX) -# define PROBE_2 /* GPIO_EMC_08 */ (GPIO_PORT4 | GPIO_PIN8 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | PROBE_IOMUX) -# define PROBE_3 /* GPIO_EMC_10 */ (GPIO_PORT4 | GPIO_PIN10 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | PROBE_IOMUX) -# define PROBE_4 /* GPIO_AD_B0_09 */ (GPIO_PORT1 | GPIO_PIN9 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | PROBE_IOMUX) -# define PROBE_5 /* GPIO_EMC_33 */ (GPIO_PORT3 | GPIO_PIN19 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | PROBE_IOMUX) -# define PROBE_6 /* GPIO_EMC_30 */ (GPIO_PORT4 | GPIO_PIN30 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | PROBE_IOMUX) -# define PROBE_7 /* GPIO_EMC_04 */ (GPIO_PORT4 | GPIO_PIN4 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | PROBE_IOMUX) -# define PROBE_8 /* GPIO_EMC_01 */ (GPIO_PORT4 | GPIO_PIN1 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | PROBE_IOMUX) +# define PROBE_1 /* GPIO_EMC_B1_23 */ (GPIO_PORT1 | GPIO_PIN23 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | PROBE_IOMUX) +# define PROBE_2 /* GPIO_EMC_B1_25 */ (GPIO_PORT1 | GPIO_PIN25 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | PROBE_IOMUX) +# define PROBE_3 /* GPIO_EMC_B1_27 */ (GPIO_PORT1 | GPIO_PIN27 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | PROBE_IOMUX) +# define PROBE_4 /* GPIO_EMC_B1_06 */ (GPIO_PORT1 | GPIO_PIN6 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | PROBE_IOMUX) +# define PROBE_5 /* GPIO_EMC_B1_08 */ (GPIO_PORT1 | GPIO_PIN8 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | PROBE_IOMUX) +# define PROBE_6 /* GPIO_EMC_B1_10 */ (GPIO_PORT1 | GPIO_PIN10 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | PROBE_IOMUX) +# define PROBE_7 /* GPIO_EMC_B1_19 */ (GPIO_PORT1 | GPIO_PIN19 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | PROBE_IOMUX) +# define PROBE_8 /* GPIO_EMC_B1_29 */ (GPIO_PORT1 | GPIO_PIN29 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | PROBE_IOMUX) +# define PROBE_9 /* GPIO_EMC_B1_31 */ (GPIO_PORT1 | GPIO_PIN31 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | PROBE_IOMUX) +# define PROBE_10 /* GPIO_EMC_B1_21 */ (GPIO_PORT1 | GPIO_PIN21 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | PROBE_IOMUX) +# define PROBE_11 /* GPIO_EMC_B1_00 */ (GPIO_PORT1 | GPIO_PIN0 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | PROBE_IOMUX) +# define PROBE_12 /* GPIO_EMC_B1_02 */ (GPIO_PORT1 | GPIO_PIN2 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | PROBE_IOMUX) # define PROBE_INIT(mask) \ do { \ @@ -313,6 +347,10 @@ if ((mask)& PROBE_N(6)) { imxrt_config_gpio(PROBE_6); } \ if ((mask)& PROBE_N(7)) { imxrt_config_gpio(PROBE_7); } \ if ((mask)& PROBE_N(8)) { imxrt_config_gpio(PROBE_8); } \ + if ((mask)& PROBE_N(9)) { imxrt_config_gpio(PROBE_9); } \ + if ((mask)& PROBE_N(10)) { imxrt_config_gpio(PROBE_10); } \ + if ((mask)& PROBE_N(11)) { imxrt_config_gpio(PROBE_11); } \ + if ((mask)& PROBE_N(12)) { imxrt_config_gpio(PROBE_12); } \ } while(0) # define PROBE(n,s) do {imxrt_gpio_write(PROBE_##n,(s));}while(0) diff --git a/boards/px4/fmu-v6xrt/nuttx-config/nsh/defconfig b/boards/px4/fmu-v6xrt/nuttx-config/nsh/defconfig index 4769e41b3290..98431d0420b0 100644 --- a/boards/px4/fmu-v6xrt/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v6xrt/nuttx-config/nsh/defconfig @@ -51,7 +51,7 @@ CONFIG_DEBUG_SYMBOLS=y CONFIG_DEBUG_TCBINFO=y CONFIG_DEV_FIFO_SIZE=0 CONFIG_DEV_PIPE_SIZE=70 -CONFIG_ETH0_PHY_LAN8742A=y +CONFIG_ETH0_PHY_MULTI=y CONFIG_FAT_DMAMEMORY=y CONFIG_FAT_LCNAMES=y CONFIG_FAT_LFN=y @@ -185,6 +185,7 @@ CONFIG_LPUART8_TXDMA=y CONFIG_MEMSET_64BIT=y CONFIG_MEMSET_OPTSPEED=y CONFIG_MMCSD=y +CONFIG_MMCSD_MULTIBLOCK_LIMIT=1 CONFIG_MMCSD_SDIO=y CONFIG_MTD=y CONFIG_MTD_BYTE_WRITE=y @@ -256,7 +257,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y CONFIG_SCHED_INSTRUMENTATION_SWITCH=y CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORKPRIORITY=50 -CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_LPWORKSTACKSIZE=2032 CONFIG_SCHED_WAITPID=y CONFIG_SDIO_BLOCKSETUP=y CONFIG_SEM_PREALLOCHOLDERS=32 diff --git a/boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld b/boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld index db67b07145eb..5951ceed7742 100644 --- a/boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld +++ b/boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld @@ -1,4 +1,66 @@ +/* Static */ +*(.text.arm_ack_irq) +*(.text.arm_doirq) +*(.text.arm_svcall) +*(.text.arm_switchcontext) +*(.text.board_autoled_on) +*(.text.clock_timer) +*(.text.exception_common) *(.text.hrt_absolute_time) +*(.text.hrt_tim_isr) +*(.text.imxrt_configwaitints) +*(.text.imxrt_dma_callback) +*(.text.imxrt_dmach_interrupt) +*(.text.imxrt_dmaterminate) +*(.text.imxrt_edma_interrupt) +*(.text.imxrt_endwait) +*(.text.imxrt_gpio3_16_31_interrupt) +*(.text.imxrt_interrupt) +*(.text.imxrt_lpi2c_isr) +*(.text.imxrt_recvdma) +*(.text.imxrt_tcd_free) +*(.text.imxrt_timerisr) +*(.text.imxrt_usbinterrupt) +*(.text.irq_dispatch) +*(.text.memcpy) +*(.text.nxsched_add_blocked) +*(.text.nxsched_add_prioritized) +*(.text.nxsched_add_readytorun) +*(.text.nxsched_get_files) +*(.text.nxsched_get_tcb) +*(.text.nxsched_merge_pending) +*(.text.nxsched_process_timer) +*(.text.nxsched_remove_blocked) +*(.text.nxsched_remove_readytorun) +*(.text.nxsched_resume_scheduler) +*(.text.nxsched_suspend_scheduler) +*(.text.nxsem_add_holder) +*(.text.nxsem_add_holder_tcb) +*(.text.nxsem_clockwait) +*(.text.nxsem_foreachholder) +*(.text.nxsem_freecount0holder) +*(.text.nxsem_freeholder) +*(.text.nxsem_post) +*(.text.nxsem_release_holder) +*(.text.nxsem_restore_baseprio) +*(.text.nxsem_tickwait) +*(.text.nxsem_timeout) +*(.text.nxsem_trywait) +*(.text.nxsem_wait) +*(.text.nxsem_wait_uninterruptible) +*(.text.nxsig_timedwait) +*(.text.sched_lock) +*(.text.sched_note_resume) +*(.text.sched_note_suspend) +*(.text.sched_unlock) +*(.text.sq_addafter) +*(.text.sq_addlast) +*(.text.sq_rem) +*(.text.sq_remafter) +*(.text.sq_remfirst) +*(.text.uart_connected) +*(.text.wd_timer) +/* Auto-generated */ *(.text._ZN4uORB7Manager27orb_add_internal_subscriberE6ORB_IDhPj) *(.text._ZN13MavlinkStream6updateERKy) *(.text._ZN7Mavlink16update_rate_multEv) @@ -12,20 +74,16 @@ *(.text._ZN4uORB12Subscription9subscribeEv.part.0) *(.text._ZN4uORB7Manager13orb_data_copyEPvS1_Rjb) *(.text._ZN4uORB10DeviceNode5writeEP4filePKcj) -*(.text.exception_common) *(.text.strcmp) *(.text._ZN4uORB10DeviceNode7publishEPK12orb_metadataPvPKv) *(.text._ZN4uORB12DeviceMaster19getDeviceNodeLockedEPK12orb_metadatah) *(.text._Z12get_orb_meta6ORB_ID) -*(.text.nxsem_wait) *(.text._ZN9ICM42688P12ProcessAccelERKyPKN20InvenSense_ICM42688P4FIFO4DATAEh) -*(.text.nxsem_post) *(.text._ZN3px49WorkQueue3RunEv) *(.text._ZN9ICM42688P11ProcessGyroERKyPKN20InvenSense_ICM42688P4FIFO4DATAEh) *(.text._ZN4EKF23RunEv) *(.text.imxrt_lpspi_exchange) *(.text.imxrt_dmach_xfrsetup) -*(.text.arm_doirq) *(.text._ZN7sensors10VehicleIMU7PublishEv) *(.text._ZN4math17WelfordMeanVectorIfLj3EE6updateERKN6matrix6VectorIfLj3EEE) *(.text._ZN7sensors10VehicleIMU10UpdateGyroEv) @@ -40,16 +98,11 @@ *(.text.perf_set_elapsed.part.0) *(.text._ZN4uORB12Subscription6updateEPv) *(.text._ZN12PX4Gyroscope10updateFIFOER18sensor_gyro_fifo_s) -*(.text.hrt_tim_isr) -*(.text.nxsig_timedwait) -*(.text.nxsem_foreachholder) *(.text._ZN7sensors10VehicleIMU3RunEv) *(.text.up_unblock_task) *(.text.__aeabi_l2f) *(.text._ZN39ControlAllocationSequentialDesaturation23computeDesaturationGainERKN6matrix6VectorIfLj16EEES4_) -*(.text.sched_unlock) *(.text.pthread_mutex_timedlock) -*(.text.nxsem_restore_baseprio) *(.text._ZN7sensors22VehicleAngularVelocity21FilterAngularVelocityEiPfi) *(.text._ZN26MulticopterAttitudeControl3RunEv.part.0) *(.text._ZN6device3SPI9_transferEPhS1_j) @@ -59,12 +112,10 @@ *(.text.fs_getfilep) *(.text.MEM_DataCopy0_1) *(.text._ZN7sensors19VehicleAcceleration3RunEv) -*(.text.sched_note_resume) *(.text.uart_ioctl) *(.text._ZN26MulticopterPositionControl3RunEv.part.0) *(.text.pthread_mutex_take) *(.text._ZN14ImuDownSampler6updateERKN9estimator9imuSampleE) -*(.text.irq_dispatch) *(.text._ZN39ControlAllocationSequentialDesaturation6mixYawEv) *(.text._ZN16ControlAllocator25publish_actuator_controlsEv.part.0) *(.text._ZN9ICM42688P7RunImplEv) @@ -74,39 +125,27 @@ *(.text._ZN7sensors22VehicleAngularVelocity21SensorSelectionUpdateERKyb) *(.text._ZN3px49WorkQueue3AddEPNS_8WorkItemE) *(.text.wd_start) -*(.text.sq_rem) -*(.text.nxsem_add_holder_tcb) -*(.text.imxrt_dmaterminate) *(.text.hrt_call_enter) *(.text._ZN4EKF220PublishLocalPositionERKy) *(.text._mav_finalize_message_chan_send) -*(.text.nxsched_add_blocked) -*(.text.arm_switchcontext) *(.text._ZN3Ekf19fixCovarianceErrorsEb) -*(.text.nxsched_add_prioritized) *(.text._ZN7sensors22VehicleAngularVelocity16ParametersUpdateEb) *(.text.ioctl) *(.text._ZN6events12SendProtocol6updateERKy) -*(.text.imxrt_dmach_interrupt) -*(.text.sched_lock) *(.text._ZN6device3SPI8transferEPhS1_j) *(.text._ZN27MavlinkStreamDistanceSensor4sendEv) *(.text.hrt_call_internal) -*(.text.arm_svcall) *(.text._ZN39ControlAllocationSequentialDesaturation18mixAirmodeDisabledEv) *(.text._ZN7Mavlink15get_free_tx_bufEv) *(.text.nx_poll) -*(.text.sched_note_suspend) *(.text._ZN15MavlinkReceiver3runEv) *(.text._ZN9ICM42688P18ProcessTemperatureEPKN20InvenSense_ICM42688P4FIFO4DATAEh) *(.text._ZN15OutputPredictor19correctOutputStatesEyRKN6matrix10QuaternionIfEERKNS0_7Vector3IfEES8_S8_S8_) *(.text._ZN3Ekf12predictStateERKN9estimator9imuSampleE) *(.text._ZN3px46logger6Logger3runEv) -*(.text.nxsem_freecount0holder) *(.text._ZN4uORB20SubscriptionInterval7updatedEv) *(.text._ZN24MavlinkStreamCommandLong4sendEv) *(.text._ZN9Commander3runEv) -*(.text.nxsem_release_holder) *(.text._ZN3Ekf17predictCovarianceERKN9estimator9imuSampleE) *(.text.wd_cancel) *(.text._ZN7Sensors3RunEv) @@ -123,16 +162,13 @@ *(.text._ZN16ControlAllocator32publish_control_allocator_statusEi) *(.text.__ieee754_atan2f) *(.text._ZNK18DynamicSparseLayer3getEt) -*(.text.nxsched_remove_readytorun) *(.text.__udivmoddi4) *(.text._ZN8Failsafe17checkStateAndModeERKyRKN12FailsafeBase5StateERK16failsafe_flags_s) *(.text._ZN29MavlinkStreamHygrometerSensor4sendEv) -*(.text.nxsched_remove_blocked) *(.text.pthread_mutex_give) *(.text._ZN3Ekf18controlFusionModesERKN9estimator9imuSampleE) *(.text._ZN4cdev4CDev11poll_notifyEm) *(.text.file_vioctl) -*(.text.wd_timer) *(.text._ZN7sensors18VotedSensorsUpdate11sensorsPollER17sensor_combined_s) *(.text.nxsig_nanosleep) *(.text.imxrt_lpspi1select) @@ -148,7 +184,6 @@ *(.text.cdcuart_ioctl) *(.text.cdcacm_sndpacket) *(.text._ZN7sensors22VehicleAngularVelocity16SensorBiasUpdateEb) -*(.text.nxsched_merge_pending) *(.text._ZN13MavlinkStream11update_dataEv) *(.text._ZN7sensors18VotedSensorsUpdate21calcGyroInconsistencyEv) *(.text.param_set_used) @@ -162,18 +197,14 @@ *(.text._ZN22MavlinkStreamCollision4sendEv) *(.text.imxrt_lpi2c_transfer) *(.text.uart_putxmitchar) -*(.text.nxsem_tickwait) *(.text.clock_nanosleep) -*(.text.memcpy) *(.text.up_release_pending) *(.text.MEM_DataCopy0) *(.text._ZN22MavlinkStreamGPSRawInt4sendEv) *(.text.dq_rem) *(.text._ZN15GyroCalibration3RunEv.part.0) -*(.text.imxrt_edma_interrupt) *(.text._ZN7sensors18VotedSensorsUpdate22calcAccelInconsistencyEv) *(.text._ZN24MavlinkStreamADSBVehicle4sendEv) -*(.text.nxsched_process_timer) *(.text.sinf) *(.text.hrt_call_after) *(.text._ZN39ControlAllocationSequentialDesaturation8allocateEv) @@ -184,8 +215,6 @@ *(.text._ZN20MavlinkStreamESCInfo4sendEv) *(.text.sem_post) *(.text._ZN3px417ScheduledWorkItem15ScheduleDelayedEm) -*(.text.nxsched_resume_scheduler) -*(.text.nxsched_add_readytorun) *(.text._ZN10FlightTaskC1Ev) *(.text.usleep) *(.text._ZN14FlightTaskAutoC1Ev) @@ -194,7 +223,6 @@ *(.text.imxrt_gpio_write) *(.text._ZN3Ekf6updateEv) *(.text.__ieee754_acosf) -*(.text.nxsem_wait_uninterruptible) *(.text._ZN3Ekf20updateIMUBiasInhibitERKN9estimator9imuSampleE) *(.text._ZN9Commander13dataLinkCheckEv) *(.text._ZN17FlightModeManager10switchTaskE15FlightTaskIndex) @@ -204,10 +232,8 @@ *(.text._ZN18MavlinkStreamDebug4sendEv) *(.text._ZN27MavlinkStreamServoOutputRawILi0EE4sendEv) *(.text.asinf) -*(.text.nxsem_freeholder) *(.text._ZN6matrix5EulerIfEC1ERKNS_3DcmIfEE) *(.text._ZN4EKF227PublishInnovationTestRatiosERKy) -*(.text.imxrt_gpio3_16_31_interrupt) *(.text._ZN4EKF213PublishStatusERKy) *(.text._ZN4EKF226PublishInnovationVariancesERKy) *(.text._ZN13land_detector23MulticopterLandDetector25_get_ground_contact_stateEv) @@ -222,7 +248,6 @@ *(.text._ZNK10ConstLayer3getEt) *(.text.__aeabi_uldivmod) *(.text.up_udelay) -*(.text.imxrt_usbinterrupt) *(.text.up_idle) *(.text._ZN20MavlinkStreamGPS2Raw4sendEv) *(.text._ZN4EKF217UpdateCalibrationERKyRNS_19InFlightCalibrationERKN6matrix7Vector3IfEES8_fbb) @@ -268,11 +293,9 @@ *(.text._ZN36MavlinkStreamPositionTargetGlobalInt4sendEv) *(.text._ZN4uORB12Subscription4copyEPv) *(.text._ZN7sensors19VehicleAcceleration21SensorSelectionUpdateEb) -*(.text.nxsem_add_holder) *(.text.crc_accumulate) *(.text._ZN3px46logger6Logger13update_paramsEv) *(.text._ZN11calibration14DeviceExternalEm) -*(.text.sq_addafter) *(.text._ZN25MavlinkStreamHomePosition8get_sizeEv) *(.text.imxrt_lpspi_modifyreg32) *(.text._ZN7sensors19VehicleAcceleration16SensorBiasUpdateEb) @@ -280,7 +303,6 @@ *(.text._ZNK6matrix6MatrixIfLj3ELj1EEmlEf) *(.text._ZN6matrix5EulerIfEC1ERKNS_10QuaternionIfEE) *(.text.imxrt_queuedtd) -*(.text.nxsched_suspend_scheduler) *(.text._ZN27MavlinkStreamDistanceSensor8get_sizeEv) *(.text._ZN3Ekf16fuseVelPosHeightEffi) *(.text._ZN3Ekf23controlBaroHeightFusionEv) @@ -294,7 +316,6 @@ *(.text._ZN7sensors14VehicleAirData3RunEv) *(.text.perf_count) *(.text._ZN3Ekf16controlMagFusionEv) -*(.text.nxsem_clockwait) *(.text.pthread_sem_give) *(.text._ZN7sensors10VehicleIMU16ParametersUpdateEb) *(.text._ZN30MavlinkStreamUTMGlobalPosition4sendEv) @@ -302,13 +323,11 @@ *(.text._ZN12I2CSPIDriverI9ICM42688PE3RunEv) *(.text._ZN17ObstacleAvoidanceC1EP12ModuleParams) *(.text.imxrt_epcomplete.constprop.0) -*(.text.imxrt_tcd_free) *(.text._ZNK6matrix6MatrixIfLj3ELj1EEmiERKS1_) *(.text._ZN9Commander30handleModeIntentionAndFailsafeEv) *(.text.perf_event_count) *(.text._ZN4EKF215PublishAttitudeERKy) *(.text._ZN19MavlinkStreamRawRpm8get_sizeEv) -*(.text.nxsem_trywait) *(.text._ZNK3px46atomicIbE4loadEv) *(.text._ZN29MavlinkStreamHygrometerSensor8get_sizeEv) *(.text.pthread_mutex_add) @@ -332,7 +351,6 @@ *(.text._ZN3Ekf31checkVerticalAccelerationHealthERKN9estimator9imuSampleE) *(.text._ZN6matrix6MatrixIfLj3ELj1EEC1ERKS1_) *(.text.udp_pollsetup) -*(.text.nxsem_timeout) *(.text._ZL14timer_callbackPv) *(.text._ZN3Ekf4fuseERKN6matrix6VectorIfLj23EEEf) *(.text._ZN13land_detector23MulticopterLandDetector22_set_hysteresis_factorEi) @@ -396,7 +414,6 @@ *(.text._ZN17MavlinkLogHandler4sendEv) *(.text._ZN7control10SuperBlock5setDtEf) *(.text._ZN29MavlinkStreamMountOrientation8get_sizeEv) -*(.text.board_autoled_on) *(.text._ZN5PX4IO13io_get_statusEv) *(.text._ZN26MulticopterAttitudeControl3RunEv) *(.text._ZThn16_N31ActuatorEffectivenessMultirotor22getEffectivenessMatrixERN21ActuatorEffectiveness13ConfigurationE25EffectivenessUpdateReason) @@ -432,7 +449,6 @@ *(.text._ZN36MavlinkStreamGimbalDeviceSetAttitude4sendEv) *(.text._ZN16PreFlightChecker6updateEfRK23estimator_innovations_s) *(.text._ZN4math13expo_deadzoneIfEEKT_RS2_S3_S3_.isra.0) -*(.text.nxsched_get_tcb) *(.text._ZN19StickAccelerationXYC1EP12ModuleParams) *(.text.imxrt_epsubmit) *(.text._ZN15PositionControl6updateEf) @@ -455,7 +471,6 @@ *(.text._ZN6Sticks25checkAndUpdateStickInputsEv) *(.text.atan2f) *(.text._ZN23MavlinkStreamRCChannels4sendEv) -*(.text.sq_remfirst) *(.text._ZN4EKF221UpdateExtVisionSampleER17ekf2_timestamps_s) *(.text.imxrt_dmach_stop) *(.text._ZN9Commander16handleAutoDisarmEv) @@ -488,7 +503,6 @@ *(.text._ZN33FlightTaskManualAltitudeSmoothVelC1Ev) *(.text.powf) *(.text._ZN4EKF217PublishEventFlagsERKy) -*(.text.sq_remafter) *(.text._ZN17FlightTaskDescend6updateEv) *(.text.imxrt_iomux_configure) *(.text.hrt_store_absolute_time) @@ -593,7 +607,6 @@ *(.text._ZN20MavlinkStreamESCInfo8get_sizeEv) *(.text._ZNK6matrix6VectorIfLj2EE4normEv) *(.text._Z15arm_auth_updateyb) -*(.text.imxrt_lpi2c_isr) *(.text._ZN3LED5ioctlEP4fileim) *(.text._ZNK3px46logger9LogWriter20had_file_write_errorEv) *(.text._ZN29MavlinkStreamLocalPositionNED4sendEv) @@ -627,13 +640,11 @@ *(.text._ZN4EKF216PublishEvPosBiasERKy) *(.text._ZN21MavlinkStreamAttitude8get_sizeEv) *(.text._ZThn16_N7sensors19VehicleAcceleration3RunEv) -*(.text.imxrt_timerisr) *(.text._ZN3Ekf24controlRangeHeightFusionEv) *(.text._ZN33MavlinkStreamTimeEstimateToTarget4sendEv) *(.text._ZN6matrix6MatrixIfLj3ELj1EE6setAllEf) *(.text._ZN12ModuleParamsD1Ev) *(.text._ZN3Ekf20controlFakeHgtFusionEv) -*(.text.sq_addlast) *(.text.imxrt_reqcomplete) *(.text._ZNK6matrix7Vector3IfEmlEf) *(.text._ZN18ZeroVelocityUpdate6updateER3EkfRKN9estimator9imuSampleE) @@ -653,7 +664,6 @@ *(.text._ZN13BatteryChecks14checkAndReportERK7ContextR6Report) *(.text._ZN18DataValidatorGroup16get_sensor_stateEj) *(.text.uart_xmitchars_done) -*(.text.nxsched_get_files) *(.text._ZN4EKF225PublishYawEstimatorStatusERKy) *(.text.sin) *(.text._ZN16PreFlightChecker27preFlightCheckVertVelFailedERK23estimator_innovations_sf) @@ -702,7 +712,6 @@ *(.text._ZThn8_N3ADC3RunEv) *(.text._ZN11StickTiltXYC1EP12ModuleParams) *(.text._ZN12SafetyButton3RunEv) -*(.text.arm_ack_irq) *(.text._ZN6BMP38811set_op_modeEh) *(.text._ZN3GPS8callbackE15GPSCallbackTypePviS1_) *(.text._ZN13AnalogBattery19get_current_channelEv) diff --git a/boards/px4/fmu-v6xrt/rover.px4board b/boards/px4/fmu-v6xrt/rover.px4board new file mode 100644 index 000000000000..2e75c9bdb47b --- /dev/null +++ b/boards/px4/fmu-v6xrt/rover.px4board @@ -0,0 +1,17 @@ +CONFIG_MODULES_AIRSPEED_SELECTOR=n +CONFIG_MODULES_FLIGHT_MODE_MANAGER=n +CONFIG_MODULES_FW_ATT_CONTROL=n +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n +CONFIG_MODULES_FW_POS_CONTROL=n +CONFIG_MODULES_FW_RATE_CONTROL=n +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n +CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n +CONFIG_MODULES_MC_ATT_CONTROL=n +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n +CONFIG_MODULES_MC_POS_CONTROL=n +CONFIG_MODULES_MC_RATE_CONTROL=n +CONFIG_MODULES_VTOL_ATT_CONTROL=n +CONFIG_EKF2_AUX_GLOBAL_POSITION=y +# CONFIG_EKF2_WIND is not set +CONFIG_MODULES_DIFFERENTIAL_DRIVE=y diff --git a/boards/px4/fmu-v6xrt/src/CMakeLists.txt b/boards/px4/fmu-v6xrt/src/CMakeLists.txt index cd9d6fb6b277..e34b9a134c72 100644 --- a/boards/px4/fmu-v6xrt/src/CMakeLists.txt +++ b/boards/px4/fmu-v6xrt/src/CMakeLists.txt @@ -36,7 +36,6 @@ if("${PX4_BOARD_LABEL}" STREQUAL "bootloader") bootloader_main.c init.c usb.c - imxrt_romapi.c imxrt_flexspi_nor_boot.c imxrt_flexspi_nor_flash.c imxrt_clockconfig.c @@ -48,6 +47,7 @@ if("${PX4_BOARD_LABEL}" STREQUAL "bootloader") nuttx_drivers # sdio px4_layer #gpio arch_io_pins # iotimer + arch_board_romapi bootloader ) target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader/common) @@ -66,13 +66,11 @@ else() i2c.cpp init.c led.c - manifest.c mtd.cpp sdhc.c spi.cpp timer_config.cpp usb.c - imxrt_romapi.c imxrt_flexspi_fram.c imxrt_flexspi_nor_boot.c imxrt_flexspi_nor_flash.c @@ -83,6 +81,7 @@ else() target_link_libraries(drivers_board PRIVATE arch_board_hw_info + arch_board_romapi arch_spi drivers__led # drv_led_start nuttx_arch # sdio diff --git a/boards/px4/fmu-v6xrt/src/board_config.h b/boards/px4/fmu-v6xrt/src/board_config.h index 82b7e4b2e09e..a2edff8dabc8 100644 --- a/boards/px4/fmu-v6xrt/src/board_config.h +++ b/boards/px4/fmu-v6xrt/src/board_config.h @@ -288,7 +288,7 @@ /* HW Version and Revision drive signals Default to 1 to detect */ -#define BOARD_HAS_HW_VERSIONING +#define BOARD_HAS_HW_SPLIT_VERSIONING #define HW_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_PULL_NONE | IOMUX_SLEW_FAST) @@ -296,7 +296,10 @@ #define GPIO_HW_REV_SENSE /* GPIO_AD_22 GPIO9 Pin 21 */ ADC_GPIO(4, 21) #define GPIO_HW_VER_SENSE /* GPIO_AD_23 GPIO9 Pin 22 */ ADC_GPIO(5, 22) #define HW_INFO_INIT_PREFIX "V6XRT" -#define V6XRT_00 HW_VER_REV(0x0,0x0) // First Release + +#define BOARD_NUM_SPI_CFG_HW_VERSIONS 2 // Rev 0 & 1 +#define V6XRT_0 HW_FMUM_ID(0x0) // First Release +#define V6XRT_1 HW_FMUM_ID(0x1) // Next Release #define BOARD_I2C_LATEINIT 1 /* See Note about SE550 Eanable */ @@ -385,7 +388,7 @@ #define GPIO_NFC_GPIO /* GPIO_EMC_B1_04 GPIO1_IO04 */ (GPIO_PORT1 | GPIO_PIN4 | GPIO_INPUT | GENERAL_INPUT_IOMUX) -#define GPIO_GPIO_EMC_B2_12 /* GPIO_EMC_B2_12 AKA PD15, PH11 */ (GPIO_PORT2 | GPIO_PIN22 | GPIO_INPUT | GENERAL_INPUT_IOMUX) +#define GPIO_GPIO_EMC_B2_12 /* GPIO_EMC_B2_12 AKA PD15, PH11 */ (GPIO_PORT2 | GPIO_PIN22 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | OUT_IOMUX) /* 10/100 Mbps Ethernet & Gigabit Ethernet */ @@ -448,8 +451,10 @@ #define HRT_PPM_CHANNEL /* GPIO_EMC_B1_09 GPIO_GPT5_CAPTURE1_1 */ 1 /* use capture/compare channel 1 */ #define GPIO_PPM_IN /* GPIO_EMC_B1_09 GPT1_CAPTURE2 */ (GPIO_GPT5_CAPTURE1_1 | GENERAL_INPUT_IOMUX) -#define RC_SERIAL_PORT "/dev/ttyS4" -#define RC_SERIAL_SINGLEWIRE +#define RC_SERIAL_PORT "/dev/ttyS4" +#define RC_SERIAL_SINGLEWIRE 1 // Suport Single wire wiring +#define RC_SERIAL_SWAP_RXTX 1 // Set Swap (but not supported in HW) to use Single wire +#define RC_SERIAL_SWAP_USING_SINGLEWIRE 1 // Set to use Single wire swap as HW does not support swap /* FLEXSPI4 */ @@ -539,6 +544,8 @@ /* This board provides the board_on_reset interface */ +#define BOARD_HAS_ISP_BOOTLOADER 1 + #define BOARD_HAS_ON_RESET 1 #define PX4_GPIO_INIT_LIST { \ diff --git a/boards/px4/fmu-v6xrt/src/imxrt_flexspi_nor_flash.c b/boards/px4/fmu-v6xrt/src/imxrt_flexspi_nor_flash.c index eb079e200934..6ae7589d60ef 100644 --- a/boards/px4/fmu-v6xrt/src/imxrt_flexspi_nor_flash.c +++ b/boards/px4/fmu-v6xrt/src/imxrt_flexspi_nor_flash.c @@ -22,7 +22,7 @@ * Included Files ****************************************************************************/ -#include "imxrt_flexspi_nor_flash.h" +#include /**************************************************************************** * Public Data diff --git a/boards/px4/fmu-v6xrt/src/init.c b/boards/px4/fmu-v6xrt/src/init.c index daac8cae427e..4b2edbc658ff 100644 --- a/boards/px4/fmu-v6xrt/src/init.c +++ b/boards/px4/fmu-v6xrt/src/init.c @@ -66,8 +66,7 @@ #include "arm_internal.h" #include "imxrt_flexspi_nor_boot.h" -#include "imxrt_flexspi_nor_flash.h" -#include "imxrt_romapi.h" +#include #include "imxrt_iomuxc.h" #include "imxrt_flexcan.h" #include "imxrt_enet.h" @@ -79,10 +78,12 @@ #include + #include #include #include #include +#include #include #include #include diff --git a/boards/px4/fmu-v6xrt/src/manifest.c b/boards/px4/fmu-v6xrt/src/manifest.c deleted file mode 100644 index 6b1a2a1695b6..000000000000 --- a/boards/px4/fmu-v6xrt/src/manifest.c +++ /dev/null @@ -1,148 +0,0 @@ -/**************************************************************************** - * - * 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 "systemlib/px4_macros.h" -#include "px4_log.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_V00[] = { - { - // PX4_MFT_PX4IO - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, - { - // PX4_MFT_USB - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, - { - // PX4_MFT_CAN2 - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, - { - // PX4_MFT_CAN3 - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, -}; - -static px4_hw_mft_list_entry_t mft_lists[] = { - {V6XRT_00, hw_mft_list_V00, arraySize(hw_mft_list_V00)}, -}; - -/************************************************************************************ - * Name: board_query_manifest - * - * Description: - * Optional returns manifest item. - * - * Input Parameters: - * manifest_id - the ID for the manifest item to retrieve - * - * Returned Value: - * 0 - item is not in manifest => assume legacy operations - * pointer to a manifest item - * - ************************************************************************************/ - -__EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id) -{ - static px4_hw_mft_list_entry boards_manifest = px4_hw_mft_list_uninitialized; - - if (boards_manifest == px4_hw_mft_list_uninitialized) { - uint32_t ver_rev = board_get_hw_version() << 8; - ver_rev |= board_get_hw_revision(); - - for (unsigned i = 0; i < arraySize(mft_lists); i++) { - if (mft_lists[i].hw_ver_rev == ver_rev) { - boards_manifest = &mft_lists[i]; - break; - } - } - - if (boards_manifest == px4_hw_mft_list_uninitialized) { - PX4_ERR("Board %4" PRIx32 " is not supported!", 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/px4/fmu-v6xrt/src/mtd.cpp b/boards/px4/fmu-v6xrt/src/mtd.cpp index 9cd1795a519a..38fa63a7eda3 100644 --- a/boards/px4/fmu-v6xrt/src/mtd.cpp +++ b/boards/px4/fmu-v6xrt/src/mtd.cpp @@ -31,6 +31,9 @@ * ****************************************************************************/ +#include +#include + #include #include @@ -120,10 +123,15 @@ static const px4_mft_entry_s mtd_mft = { .pmft = (void *) &board_mtd_config, }; +static const px4_mft_entry_s mft_mft = { + .type = MFT, + .pmft = (void *) system_query_manifest, +}; static const px4_mft_s mft = { - .nmft = 1, + .nmft = 2, .mfts = { - &mtd_mft + &mtd_mft, + &mft_mft, } }; diff --git a/boards/px4/fmu-v6xrt/src/spi.cpp b/boards/px4/fmu-v6xrt/src/spi.cpp index 16c3e2822dc6..d7e05735cd5a 100644 --- a/boards/px4/fmu-v6xrt/src/spi.cpp +++ b/boards/px4/fmu-v6xrt/src/spi.cpp @@ -41,47 +41,46 @@ #include #include -#include -#include +constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSIONS] = { + initSPIFmumID(V6XRT_0, { + initSPIBus(SPI::Bus::LPSPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42686P, SPI::CS{GPIO::Port2, GPIO::Pin11}, SPI::DRDY{GPIO::Port3, GPIO::Pin19}), /* GPIO_EMC_B2_01 GPIO2_IO11, GPIO_AD_20, GPIO3_IO19 */ + }, {GPIO::Port2, GPIO::Pin1}), // Power GPIO_EMC_B1_33 GPIO2_IO01 -#include -#include -#include + initSPIBus(SPI::Bus::LPSPI2, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::Port3, GPIO::Pin24}, SPI::DRDY{GPIO::Port2, GPIO::Pin7}), /* GPIO_AD_25 GPIO3_IO24, GPIO_EMC_B1_39 GPIO2_IO07 */ + }, {GPIO::Port1, GPIO::Pin22}), // Power GPIO_EMC_B1_22 GPIO1_IO22 -#include -#include -#include -#include - -#include -#include -#include "imxrt_lpspi.h" -#include "imxrt_gpio.h" -#include "board_config.h" -#include - -#ifdef CONFIG_IMXRT_LPSPI + initSPIBus(SPI::Bus::LPSPI3, { + initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::Port2, GPIO::Pin18}, SPI::DRDY{GPIO::Port2, GPIO::Pin28}), /* GPIO_EMC_B2_08 GPIO2_IO18, GPIO_EMC_B2_18 GPIO2_IO28 */ + initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::Port2, GPIO::Pin15}), /* GPIO_EMC_B2_05 GPIO2_IO15 */ + }, {GPIO::Port1, GPIO::Pin14}), // Power GPIO_EMC_B1_14 GPIO1_IO14 + initSPIBusExternal(SPI::Bus::LPSPI6, { + initSPIConfigExternal(SPI::CS{GPIO::Port6, GPIO::Pin9}, SPI::DRDY{GPIO::Port1, GPIO::Pin5}), /* GPIO_LPSR_09 GPIO6_IO09 GPIO_EMC_B1_05 GPIO1_IO05*/ + initSPIConfigExternal(SPI::CS{GPIO::Port6, GPIO::Pin8}, SPI::DRDY{GPIO::Port1, GPIO::Pin7}), /* GPIO_LPSR_08 GPIO6_IO08 GPIO_EMC_B1_07 GPIO1_IO07*/ + }), + }), -constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { - initSPIBus(SPI::Bus::LPSPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM42686P, SPI::CS{GPIO::Port2, GPIO::Pin11}, SPI::DRDY{GPIO::Port3, GPIO::Pin19}), /* GPIO_EMC_B2_01 GPIO2_IO11, GPIO_AD_20, GPIO3_IO19 */ - }, {GPIO::Port2, GPIO::Pin1}), // Power GPIO_EMC_B1_33 GPIO2_IO01 + initSPIFmumID(V6XRT_1, { + initSPIBus(SPI::Bus::LPSPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42686P, SPI::CS{GPIO::Port2, GPIO::Pin11}, SPI::DRDY{GPIO::Port3, GPIO::Pin19}), /* GPIO_EMC_B2_01 GPIO2_IO11, GPIO_AD_20, GPIO3_IO19 */ + }, {GPIO::Port2, GPIO::Pin1}), // Power GPIO_EMC_B1_33 GPIO2_IO01 - initSPIBus(SPI::Bus::LPSPI2, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::Port3, GPIO::Pin24}, SPI::DRDY{GPIO::Port2, GPIO::Pin7}), /* GPIO_AD_25 GPIO3_IO24, GPIO_EMC_B1_39 GPIO2_IO07 */ - }, {GPIO::Port1, GPIO::Pin22}), // Power GPIO_EMC_B1_22 GPIO1_IO22 + initSPIBus(SPI::Bus::LPSPI2, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::Port3, GPIO::Pin24}, SPI::DRDY{GPIO::Port2, GPIO::Pin7}), /* GPIO_AD_25 GPIO3_IO24, GPIO_EMC_B1_39 GPIO2_IO07 */ + }, {GPIO::Port1, GPIO::Pin22}), // Power GPIO_EMC_B1_22 GPIO1_IO22 - initSPIBus(SPI::Bus::LPSPI3, { - initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::Port2, GPIO::Pin18}, SPI::DRDY{GPIO::Port2, GPIO::Pin28}), /* GPIO_EMC_B2_08 GPIO2_IO18, GPIO_EMC_B2_18 GPIO2_IO28 */ - initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::Port2, GPIO::Pin15}), /* GPIO_EMC_B2_05 GPIO2_IO15 */ - }, {GPIO::Port1, GPIO::Pin14}), // Power GPIO_EMC_B1_14 GPIO1_IO14 + initSPIBus(SPI::Bus::LPSPI3, { + initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::Port2, GPIO::Pin18}, SPI::DRDY{GPIO::Port2, GPIO::Pin28}), /* GPIO_EMC_B2_08 GPIO2_IO18, GPIO_EMC_B2_18 GPIO2_IO28 */ + initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::Port2, GPIO::Pin15}), /* GPIO_EMC_B2_05 GPIO2_IO15 */ + }, {GPIO::Port1, GPIO::Pin14}), // Power GPIO_EMC_B1_14 GPIO1_IO14 - initSPIBusExternal(SPI::Bus::LPSPI6, { - initSPIConfigExternal(SPI::CS{GPIO::Port6, GPIO::Pin9}, SPI::DRDY{GPIO::Port1, GPIO::Pin5}), /* GPIO_LPSR_09 GPIO6_IO09 GPIO_EMC_B1_05 GPIO1_IO05*/ - initSPIConfigExternal(SPI::CS{GPIO::Port6, GPIO::Pin8}, SPI::DRDY{GPIO::Port1, GPIO::Pin7}), /* GPIO_LPSR_08 GPIO6_IO08 GPIO_EMC_B1_07 GPIO1_IO07*/ + initSPIBusExternal(SPI::Bus::LPSPI6, { + initSPIConfigExternal(SPI::CS{GPIO::Port6, GPIO::Pin9}, SPI::DRDY{GPIO::Port1, GPIO::Pin5}), /* GPIO_LPSR_09 GPIO6_IO09 GPIO_EMC_B1_05 GPIO1_IO05*/ + initSPIConfigExternal(SPI::CS{GPIO::Port6, GPIO::Pin8}, SPI::DRDY{GPIO::Port1, GPIO::Pin7}), /* GPIO_LPSR_08 GPIO6_IO08 GPIO_EMC_B1_07 GPIO1_IO07*/ + }), }), }; -#endif -static constexpr bool unused = validateSPIConfig(px4_spi_buses); +static constexpr bool unused = validateSPIConfig(px4_spi_buses_all_hw); diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index 008dc97a1298..7dd7ce481a3f 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -12,8 +12,9 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y +CONFIG_MODULES_DIFFERENTIAL_DRIVE=y CONFIG_MODULES_EKF2=y +CONFIG_EKF2_VERBOSE_STATUS=y CONFIG_EKF2_AUX_GLOBAL_POSITION=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y diff --git a/boards/siyi/n7/nuttx-config/include/board.h b/boards/siyi/n7/nuttx-config/include/board.h index d6b690c29325..1df145ed26db 100644 --- a/boards/siyi/n7/nuttx-config/include/board.h +++ b/boards/siyi/n7/nuttx-config/include/board.h @@ -222,7 +222,7 @@ #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_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 /* ADC 1 2 3 clock source */ #define STM32_RCC_D2CCIP1R_FDCANSEL RCC_D2CCIP1R_FDCANSEL_HSE /* FDCAN 1 2 clock source */ diff --git a/boards/sky-drones/smartap-airlink/src/board_config.h b/boards/sky-drones/smartap-airlink/src/board_config.h index ac21a1686cb2..5b957e0694cc 100644 --- a/boards/sky-drones/smartap-airlink/src/board_config.h +++ b/boards/sky-drones/smartap-airlink/src/board_config.h @@ -168,7 +168,7 @@ /* HW Version and Revision drive signals Default to 1 to detect */ -#define BOARD_HAS_HW_VERSIONING +#define BOARD_HAS_HW_VERSIONING // migrate to Split #define GPIO_HW_VER_REV_DRIVE /* PG0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN0) #define GPIO_HW_REV_SENSE /* PF5 */ ADC3_GPIO(15) diff --git a/boards/spracing/h7extreme/nuttx-config/include/board.h b/boards/spracing/h7extreme/nuttx-config/include/board.h index 91edb4e650c3..117edb76a749 100644 --- a/boards/spracing/h7extreme/nuttx-config/include/board.h +++ b/boards/spracing/h7extreme/nuttx-config/include/board.h @@ -241,7 +241,7 @@ /* ADC 1 2 3 clock source */ -#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2 +#define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 /* QSPI clock source */ #define STM32_RCC_D1CCIPR_QSPISEL RCC_D1CCIPR_QSPISEL_PLL2 diff --git a/boards/spracing/h7extreme/src/rcc.c b/boards/spracing/h7extreme/src/rcc.c index 9fc05848163a..cd4299b880ea 100644 --- a/boards/spracing/h7extreme/src/rcc.c +++ b/boards/spracing/h7extreme/src/rcc.c @@ -503,10 +503,10 @@ __ramfunc__ void stm32_board_clockconfig(void) /* Configure ADC source clock */ -#if defined(STM32_RCC_D3CCIPR_ADCSEL) +#if defined(STM32_RCC_D3CCIPR_ADCSRC) regval = getreg32(STM32_RCC_D3CCIPR); regval &= ~RCC_D3CCIPR_ADCSEL_MASK; - regval |= STM32_RCC_D3CCIPR_ADCSEL; + regval |= STM32_RCC_D3CCIPR_ADCSRC; putreg32(regval, STM32_RCC_D3CCIPR); #endif diff --git a/boards/thepeach/k1/default.px4board b/boards/thepeach/k1/default.px4board index b2c6588f98b2..39fefb9b68a8 100644 --- a/boards/thepeach/k1/default.px4board +++ b/boards/thepeach/k1/default.px4board @@ -69,6 +69,7 @@ CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y CONFIG_MODULES_UUV_ATT_CONTROL=y CONFIG_MODULES_UUV_POS_CONTROL=y +CONFIG_MODULES_UXRCE_DDS_CLIENT=y CONFIG_MODULES_VTOL_ATT_CONTROL=y CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y CONFIG_SYSTEMCMDS_BL_UPDATE=y diff --git a/boards/thepeach/r1/default.px4board b/boards/thepeach/r1/default.px4board index b2c6588f98b2..39fefb9b68a8 100644 --- a/boards/thepeach/r1/default.px4board +++ b/boards/thepeach/r1/default.px4board @@ -69,6 +69,7 @@ CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y CONFIG_MODULES_UUV_ATT_CONTROL=y CONFIG_MODULES_UUV_POS_CONTROL=y +CONFIG_MODULES_UXRCE_DDS_CLIENT=y CONFIG_MODULES_VTOL_ATT_CONTROL=y CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y CONFIG_SYSTEMCMDS_BL_UPDATE=y diff --git a/eclipse.cproject b/eclipse.cproject deleted file mode 100644 index 711a641270c9..000000000000 --- a/eclipse.cproject +++ /dev/null @@ -1,205 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - make - px4_fmu-v2_default - true - true - true - - - make - - px4_fmu-v4_default - true - true - true - - - make - all - true - true - true - - - make - - px4_sitl_default - true - true - true - - - make - - check - true - true - true - - - make - clean - true - true - true - - - make - distclean - true - true - true - - - make - - tests - true - true - true - - - make - submodulesclean - false - true - true - - - make - - quick_check - true - true - true - - - - - - - - - - - - - - - - - - - diff --git a/eclipse.project b/eclipse.project deleted file mode 100644 index 0172fa8ceb13..000000000000 --- a/eclipse.project +++ /dev/null @@ -1,69 +0,0 @@ - - - PX4-Firmware - - - - - - org.eclipse.cdt.managedbuilder.core.genmakebuilder - clean,full,incremental, - - - - - org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder - full,incremental, - - - - - - org.eclipse.cdt.core.cnature - org.eclipse.cdt.core.ccnature - org.eclipse.cdt.managedbuilder.core.managedBuildNature - org.eclipse.cdt.managedbuilder.core.ScannerConfigNature - - - - topics_sources - 2 - uORB_SRC - - - uORB - 2 - uORB_LOC - - - - - 1457837186676 - - 26 - - org.eclipse.ui.ide.multiFilter - 1.0-name-matches-false-false-.git - - - - 1457837186687 - - 10 - - org.eclipse.ui.ide.multiFilter - 1.0-name-matches-true-false-build* - - - - - - uORB_LOC - $%7BPROJECT_LOC%7D/build/px4_sitl_default/uORB - - - uORB_SRC - $%7BPROJECT_LOC%7D/build/px4_sitl_default/msg/topics_sources - - - diff --git a/msg/BatteryStatus.msg b/msg/BatteryStatus.msg index 65845b57aec2..66fcffa0e68f 100644 --- a/msg/BatteryStatus.msg +++ b/msg/BatteryStatus.msg @@ -4,7 +4,7 @@ float32 voltage_v # Battery voltage in volts, 0 if unknown float32 voltage_filtered_v # Battery voltage in volts, filtered, 0 if unknown float32 current_a # Battery current in amperes, -1 if unknown float32 current_filtered_a # Battery current in amperes, filtered, 0 if unknown -float32 current_average_a # Battery current average in amperes, -1 if unknown +float32 current_average_a # Battery current average in amperes (for FW average in level flight), -1 if unknown float32 discharged_mah # Discharged amount in mAh, -1 if unknown float32 remaining # From 1 to 0, -1 if unknown float32 scale # Power scaling factor, >= 1, or -1 if unknown diff --git a/msg/Buffer128.msg b/msg/Buffer128.msg new file mode 100644 index 000000000000..342aa83dbe8d --- /dev/null +++ b/msg/Buffer128.msg @@ -0,0 +1,9 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 len # length of data +uint32 MAX_BUFLEN = 128 + +uint8[128] data # data + +# TOPICS voxl2_io_data + diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 31c024081612..c53402bd0f35 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -53,10 +53,12 @@ set(msg_files ArmingCheckRequest.msg AutotuneAttitudeControlStatus.msg BatteryStatus.msg + Buffer128.msg ButtonEvent.msg CameraCapture.msg CameraStatus.msg CameraTrigger.msg + CanInterfaceStatus.msg CellularStatus.msg CollisionConstraints.msg CollisionReport.msg @@ -92,11 +94,13 @@ set(msg_files FigureEightStatus.msg FailsafeFlags.msg FailureDetectorStatus.msg + FlightPhaseEstimation.msg FollowTarget.msg FollowTargetEstimator.msg FollowTargetStatus.msg GeneratorStatus.msg GeofenceResult.msg + GeofenceStatus.msg GimbalControls.msg GimbalDeviceAttitudeStatus.msg GimbalDeviceInformation.msg @@ -151,6 +155,10 @@ set(msg_files OrbTest.msg OrbTestLarge.msg OrbTestMedium.msg + ParameterResetRequest.msg + ParameterSetUsedRequest.msg + ParameterSetValueRequest.msg + ParameterSetValueResponse.msg ParameterUpdate.msg Ping.msg PositionControllerLandingStatus.msg @@ -171,6 +179,7 @@ set(msg_files RegisterExtComponentReply.msg RegisterExtComponentRequest.msg Rpm.msg + RtlStatus.msg RtlTimeEstimate.msg SatelliteInfo.msg SensorAccel.msg @@ -191,6 +200,7 @@ set(msg_files SensorsStatus.msg SensorsStatusImu.msg SensorUwb.msg + SensorAirflow.msg SystemPower.msg TakeoffStatus.msg TaskStackInfo.msg @@ -280,6 +290,9 @@ foreach(msg_file ${msg_files}) list(APPEND uorb_json_files ${msg_source_out_path}/${msg}.json) endforeach() +# set parent scope msg_files for ROS +set(msg_files ${msg_files} PARENT_SCOPE) + # Generate uORB headers add_custom_command( OUTPUT diff --git a/msg/CanInterfaceStatus.msg b/msg/CanInterfaceStatus.msg new file mode 100644 index 000000000000..4129c8d56386 --- /dev/null +++ b/msg/CanInterfaceStatus.msg @@ -0,0 +1,6 @@ +uint64 timestamp # time since system start (microseconds) +uint8 interface + +uint64 io_errors +uint64 frames_tx +uint64 frames_rx diff --git a/msg/DifferentialDriveSetpoint.msg b/msg/DifferentialDriveSetpoint.msg index 386dc3a9d906..f7e4c5840099 100644 --- a/msg/DifferentialDriveSetpoint.msg +++ b/msg/DifferentialDriveSetpoint.msg @@ -1,4 +1,8 @@ uint64 timestamp # time since system start (microseconds) float32 speed # [m/s] collective roll-off speed in body x-axis +bool closed_loop_speed_control # true if speed is controlled using estimator feedback, false if direct feed-forward float32 yaw_rate # [rad/s] yaw rate +bool closed_loop_yaw_rate_control # true if yaw rate is controlled using gyroscope feedback, false if direct feed-forward + +# TOPICS differential_drive_setpoint differential_drive_control_output diff --git a/msg/EstimatorStatusFlags.msg b/msg/EstimatorStatusFlags.msg index b8c5d3e07379..c6e0504f158f 100644 --- a/msg/EstimatorStatusFlags.msg +++ b/msg/EstimatorStatusFlags.msg @@ -55,15 +55,9 @@ bool fs_bad_airspeed # 5 - true if fusion of the airspeed has encounte bool fs_bad_sideslip # 6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error bool fs_bad_optflow_x # 7 - true if fusion of the optical flow X axis has encountered a numerical error bool fs_bad_optflow_y # 8 - true if fusion of the optical flow Y axis has encountered a numerical error -bool fs_bad_vel_n # 9 - true if fusion of the North velocity has encountered a numerical error -bool fs_bad_vel_e # 10 - true if fusion of the East velocity has encountered a numerical error -bool fs_bad_vel_d # 11 - true if fusion of the Down velocity has encountered a numerical error -bool fs_bad_pos_n # 12 - true if fusion of the North position has encountered a numerical error -bool fs_bad_pos_e # 13 - true if fusion of the East position has encountered a numerical error -bool fs_bad_pos_d # 14 - true if fusion of the Down position has encountered a numerical error -bool fs_bad_acc_bias # 15 - true if bad delta velocity bias estimates have been detected -bool fs_bad_acc_vertical # 16 - true if bad vertical accelerometer data has been detected -bool fs_bad_acc_clipping # 17 - true if delta velocity data contains clipping (asymmetric railing) +bool fs_bad_acc_bias # 9 - true if bad delta velocity bias estimates have been detected +bool fs_bad_acc_vertical # 10 - true if bad vertical accelerometer data has been detected +bool fs_bad_acc_clipping # 11 - true if delta velocity data contains clipping (asymmetric railing) # innovation test failures diff --git a/msg/FlightPhaseEstimation.msg b/msg/FlightPhaseEstimation.msg new file mode 100644 index 000000000000..e05b3291292c --- /dev/null +++ b/msg/FlightPhaseEstimation.msg @@ -0,0 +1,8 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 flight_phase # Estimate of current flight phase + +uint8 FLIGHT_PHASE_UNKNOWN = 0 # vehicle flight phase is unknown +uint8 FLIGHT_PHASE_LEVEL = 1 # Vehicle is in level flight +uint8 FLIGHT_PHASE_DESCEND = 2 # vehicle is in descend +uint8 FLIGHT_PHASE_CLIMB = 3 # vehicle is climbing diff --git a/msg/GeofenceStatus.msg b/msg/GeofenceStatus.msg new file mode 100644 index 000000000000..d32b9010c312 --- /dev/null +++ b/msg/GeofenceStatus.msg @@ -0,0 +1,7 @@ +uint64 timestamp # time since system start (microseconds) + +uint32 geofence_id # loaded geofence id +uint8 status # Current geofence status + +uint8 GF_STATUS_LOADING = 0 +uint8 GF_STATUS_READY = 1 diff --git a/msg/Mission.msg b/msg/Mission.msg index a2479d86aaef..a923193da807 100644 --- a/msg/Mission.msg +++ b/msg/Mission.msg @@ -1,5 +1,7 @@ uint64 timestamp # time since system start (microseconds) -uint8 dataman_id # default 0, there are two offboard storage places in the dataman: 0 or 1 +uint8 mission_dataman_id # default 0, there are two offboard storage places in the dataman: 0 or 1 +uint8 fence_dataman_id # default 0, there are two offboard storage places in the dataman: 0 or 1 +uint8 safepoint_dataman_id # default 0, there are two offboard storage places in the dataman: 0 or 1 uint16 count # count of the missions stored in the dataman int32 current_seq # default -1, start at the one changed latest diff --git a/msg/MissionResult.msg b/msg/MissionResult.msg index aaa840762d99..f70326be3b26 100644 --- a/msg/MissionResult.msg +++ b/msg/MissionResult.msg @@ -1,8 +1,8 @@ uint64 timestamp # time since system start (microseconds) -uint16 mission_id # Id for the mission for which the result was generated -uint16 geofence_id # Id for the corresponding geofence for which the result was generated (used for mission feasibility) -uint64 home_position_counter # Counter of the home position for which the result was generated (used for mission feasibility) +uint32 mission_id # Id for the mission for which the result was generated +uint32 geofence_id # Id for the corresponding geofence for which the result was generated (used for mission feasibility) +uint32 home_position_counter # Counter of the home position for which the result was generated (used for mission feasibility) int32 seq_reached # Sequence of the mission item which has been reached, default -1 uint16 seq_current # Sequence of the current mission item diff --git a/msg/ModeCompleted.msg b/msg/ModeCompleted.msg index 98f331ed6455..bacff4a94b45 100644 --- a/msg/ModeCompleted.msg +++ b/msg/ModeCompleted.msg @@ -1,4 +1,5 @@ # Mode completion result, published by an active mode. +# The possible values of nav_state are defined in the VehicleStatus msg. # Note that this is not always published (e.g. when a user switches modes or on # failsafe activation) uint64 timestamp # time since system start (microseconds) @@ -10,5 +11,5 @@ uint8 RESULT_FAILURE_OTHER = 100 # Mode failed (generic error) uint8 result # One of RESULT_* -uint8 nav_state # Source mode +uint8 nav_state # Source mode (values in VehicleStatus) diff --git a/msg/OrbTestMedium.msg b/msg/OrbTestMedium.msg index b25ae1c8514b..43109d49d54e 100644 --- a/msg/OrbTestMedium.msg +++ b/msg/OrbTestMedium.msg @@ -4,4 +4,6 @@ int32 val uint8[64] junk +uint8 ORB_QUEUE_LENGTH = 16 + # TOPICS orb_test_medium orb_test_medium_multi orb_test_medium_wrap_around orb_test_medium_queue orb_test_medium_queue_poll diff --git a/msg/ParameterResetRequest.msg b/msg/ParameterResetRequest.msg new file mode 100644 index 000000000000..db08edb37961 --- /dev/null +++ b/msg/ParameterResetRequest.msg @@ -0,0 +1,8 @@ +# ParameterResetRequest : Used by the primary to reset one or all parameter value(s) on the remote + +uint64 timestamp +uint16 parameter_index + +bool reset_all # If this is true then ignore parameter_index + +uint8 ORB_QUEUE_LENGTH = 4 diff --git a/msg/ParameterSetUsedRequest.msg b/msg/ParameterSetUsedRequest.msg new file mode 100644 index 000000000000..ca97f9e9e7ff --- /dev/null +++ b/msg/ParameterSetUsedRequest.msg @@ -0,0 +1,6 @@ +# ParameterSetUsedRequest : Used by a remote to update the used flag for a parameter on the primary + +uint64 timestamp +uint16 parameter_index + +uint8 ORB_QUEUE_LENGTH = 64 diff --git a/msg/ParameterSetValueRequest.msg b/msg/ParameterSetValueRequest.msg new file mode 100644 index 000000000000..eaf650f24324 --- /dev/null +++ b/msg/ParameterSetValueRequest.msg @@ -0,0 +1,11 @@ +# ParameterSetValueRequest : Used by a remote or primary to update the value for a parameter at the other end + +uint64 timestamp +uint16 parameter_index + +int32 int_value # Optional value for an integer parameter +float32 float_value # Optional value for a float parameter + +uint8 ORB_QUEUE_LENGTH = 32 + +# TOPICS parameter_set_value_request parameter_remote_set_value_request parameter_primary_set_value_request diff --git a/msg/ParameterSetValueResponse.msg b/msg/ParameterSetValueResponse.msg new file mode 100644 index 000000000000..09f8e3084a80 --- /dev/null +++ b/msg/ParameterSetValueResponse.msg @@ -0,0 +1,9 @@ +# ParameterSetValueResponse : Response to a set value request by either primary or secondary + +uint64 timestamp +uint64 request_timestamp +uint16 parameter_index + +uint8 ORB_QUEUE_LENGTH = 4 + +# TOPICS parameter_set_value_response parameter_remote_set_value_response parameter_primary_set_value_response \ No newline at end of file diff --git a/msg/RtlStatus.msg b/msg/RtlStatus.msg new file mode 100644 index 000000000000..94c90d513e38 --- /dev/null +++ b/msg/RtlStatus.msg @@ -0,0 +1,15 @@ +uint64 timestamp # time since system start (microseconds) + +uint32 safe_points_id # unique ID of active set of safe_point_items +bool is_evaluation_pending # flag if the RTL point needs reevaluation (e.g. new safe points available, but need loading). + +bool has_vtol_approach # flag if approaches are defined for current RTL_TYPE parameter setting + +uint8 rtl_type # Type of RTL chosen +uint8 safe_point_index # index of the chosen safe point, if in RTL_STATUS_TYPE_DIRECT_SAFE_POINT mode + +uint8 RTL_STATUS_TYPE_NONE=0 # RTL type is pending if evaluation can't pe performed currently e.g. when it is still loading the safe points +uint8 RTL_STATUS_TYPE_DIRECT_SAFE_POINT=1 # RTL type is chosen to directly go to a safe point or home position +uint8 RTL_STATUS_TYPE_DIRECT_MISSION_LAND=2 # RTL type is going straight to the beginning of the mission landing +uint8 RTL_STATUS_TYPE_FOLLOW_MISSION=3 # RTL type is following the mission from closest point to mission landing +uint8 RTL_STATUS_TYPE_FOLLOW_MISSION_REVERSE=4 # RTL type is following the mission in reverse to the start position diff --git a/msg/SensorAirflow.msg b/msg/SensorAirflow.msg new file mode 100644 index 000000000000..dd55ad0b0c70 --- /dev/null +++ b/msg/SensorAirflow.msg @@ -0,0 +1,5 @@ +uint64 timestamp # time since system start (microseconds) +uint32 device_id # unique device ID for the sensor that does not change between power cycles +float32 speed # the speed being reported by the wind / airflow sensor +float32 direction # the direction bein report by the wind / airflow sensor +uint8 status # Status code from the sensor diff --git a/msg/SensorGnssRelative.msg b/msg/SensorGnssRelative.msg index 0f1736f45fd8..6d87a344c127 100644 --- a/msg/SensorGnssRelative.msg +++ b/msg/SensorGnssRelative.msg @@ -15,8 +15,8 @@ float32[3] position_accuracy # Accuracy of relative position (m) float32 heading # Heading of the relative position vector (radians) float32 heading_accuracy # Accuracy of heading of the relative position vector (radians) -float32 position_length -float32 accuracy_length +float32 position_length # Length of the position vector (m) +float32 accuracy_length # Accuracy of the position length (m) bool gnss_fix_ok # GNSS valid fix (i.e within DOP & accuracy masks) bool differential_solution # differential corrections were applied diff --git a/msg/TransponderReport.msg b/msg/TransponderReport.msg index 6a1ca064186b..d5171cf3b303 100644 --- a/msg/TransponderReport.msg +++ b/msg/TransponderReport.msg @@ -47,4 +47,4 @@ uint16 ADSB_EMITTER_TYPE_SERVICE_SURFACE=18 uint16 ADSB_EMITTER_TYPE_POINT_OBSTACLE=19 uint16 ADSB_EMITTER_TYPE_ENUM_END=20 -uint8 ORB_QUEUE_LENGTH = 8 +uint8 ORB_QUEUE_LENGTH = 16 diff --git a/msg/VehicleCommand.msg b/msg/VehicleCommand.msg index e5a9c0fdc229..801e5b279f61 100644 --- a/msg/VehicleCommand.msg +++ b/msg/VehicleCommand.msg @@ -101,6 +101,8 @@ uint16 VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002 # Control a pre-programmed pay uint16 VEHICLE_CMD_FIXED_MAG_CAL_YAW = 42006 # Magnetometer calibration based on provided known yaw. This allows for fast calibration using WMM field tables in the vehicle, given only the known yaw of the vehicle. If Latitude and longitude are both zero then use the current vehicle location. uint16 VEHICLE_CMD_DO_WINCH = 42600 # Command to operate winch. +uint16 VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE = 43003 # external reset of estimator global position when deadreckoning + # PX4 vehicle commands (beyond 16 bit mavlink commands) uint32 VEHICLE_CMD_PX4_INTERNAL_START = 65537 # start of PX4 internal only vehicle commands (> UINT16_MAX) uint32 VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN = 100000 # Sets the GPS coordinates of the vehicle local origin (0,0,0) position. |Empty|Empty|Empty|Empty|Latitude|Longitude|Altitude| diff --git a/msg/VehicleLocalPosition.msg b/msg/VehicleLocalPosition.msg index c08b4117289f..c1a0dffe14f6 100644 --- a/msg/VehicleLocalPosition.msg +++ b/msg/VehicleLocalPosition.msg @@ -38,11 +38,14 @@ 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 heading_var float32 unaided_heading # Same as heading but generated by integrating corrected gyro data only float32 delta_heading # Heading delta caused by latest heading reset [rad] uint8 heading_reset_counter # Index of latest heading reset bool heading_good_for_control +float32 tilt_var + # Position of reference point (local NED frame origin) in global (GPS / WGS84) frame bool xy_global # true if position (x, y) has a valid global reference (ref_lat, ref_lon) bool z_global # true if z has a valid global reference (ref_alt) diff --git a/platforms/common/CMakeLists.txt b/platforms/common/CMakeLists.txt index c6109decf628..cb0721fe6911 100644 --- a/platforms/common/CMakeLists.txt +++ b/platforms/common/CMakeLists.txt @@ -51,6 +51,8 @@ add_library(px4_platform STATIC px4_cli.cpp shutdown.cpp spi.cpp + pab_manifest.c + Serial.cpp ${SRCS} ) target_link_libraries(px4_platform prebuild_targets px4_work_queue) diff --git a/platforms/common/Serial.cpp b/platforms/common/Serial.cpp new file mode 100644 index 000000000000..2f93a66a6ebf --- /dev/null +++ b/platforms/common/Serial.cpp @@ -0,0 +1,138 @@ +/**************************************************************************** + * + * Copyright (C) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include + +namespace device +{ + +Serial::Serial(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits, + FlowControl flowcontrol) : + _impl(port, baudrate, bytesize, parity, stopbits, flowcontrol) +{ + // If no baudrate was specified then set it to a reasonable default value + if (baudrate == 0) { + (void) _impl.setBaudrate(9600); + } +} + +Serial::~Serial() +{ +} + +bool Serial::open() +{ + return _impl.open(); +} + +bool Serial::isOpen() const +{ + return _impl.isOpen(); +} + +bool Serial::close() +{ + return _impl.close(); +} + +ssize_t Serial::read(uint8_t *buffer, size_t buffer_size) +{ + return _impl.read(buffer, buffer_size); +} + +ssize_t Serial::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us) +{ + return _impl.readAtLeast(buffer, buffer_size, character_count, timeout_us); +} + +ssize_t Serial::write(const void *buffer, size_t buffer_size) +{ + return _impl.write(buffer, buffer_size); +} + +uint32_t Serial::getBaudrate() const +{ + return _impl.getBaudrate(); +} + +bool Serial::setBaudrate(uint32_t baudrate) +{ + return _impl.setBaudrate(baudrate); +} + +ByteSize Serial::getBytesize() const +{ + return _impl.getBytesize(); +} + +bool Serial::setBytesize(ByteSize bytesize) +{ + return _impl.setBytesize(bytesize); +} + +Parity Serial::getParity() const +{ + return _impl.getParity(); +} + +bool Serial::setParity(Parity parity) +{ + return _impl.setParity(parity); +} + +StopBits Serial::getStopbits() const +{ + return _impl.getStopbits(); +} + +bool Serial::setStopbits(StopBits stopbits) +{ + return _impl.setStopbits(stopbits); +} + +FlowControl Serial::getFlowcontrol() const +{ + return _impl.getFlowcontrol(); +} + +bool Serial::setFlowcontrol(FlowControl flowcontrol) +{ + return _impl.setFlowcontrol(flowcontrol); +} + +const char *Serial::getPort() const +{ + return _impl.getPort(); +} + +} // namespace device diff --git a/platforms/common/include/px4_platform_common/Serial.hpp b/platforms/common/include/px4_platform_common/Serial.hpp new file mode 100644 index 000000000000..ce02b5ac6308 --- /dev/null +++ b/platforms/common/include/px4_platform_common/Serial.hpp @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * Copyright (C) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +#include + +using device::SerialConfig::ByteSize; +using device::SerialConfig::Parity; +using device::SerialConfig::StopBits; +using device::SerialConfig::FlowControl; + +namespace device __EXPORT +{ + +class Serial +{ +public: + Serial(const char *port, uint32_t baudrate = 57600, + ByteSize bytesize = ByteSize::EightBits, Parity parity = Parity::None, + StopBits stopbits = StopBits::One, FlowControl flowcontrol = FlowControl::Disabled); + virtual ~Serial(); + + // Open sets up the port and gets it configured based on desired configuration + bool open(); + bool isOpen() const; + + bool close(); + + ssize_t read(uint8_t *buffer, size_t buffer_size); + ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0); + + ssize_t write(const void *buffer, size_t buffer_size); + + // If port is already open then the following configuration functions + // will reconfigure the port. If the port is not yet open then they will + // simply store the configuration in preparation for the port to be opened. + + uint32_t getBaudrate() const; + bool setBaudrate(uint32_t baudrate); + + ByteSize getBytesize() const; + bool setBytesize(ByteSize bytesize); + + Parity getParity() const; + bool setParity(Parity parity); + + StopBits getStopbits() const; + bool setStopbits(StopBits stopbits); + + FlowControl getFlowcontrol() const; + bool setFlowcontrol(FlowControl flowcontrol); + + const char *getPort() const; + +private: + // Disable copy constructors + Serial(const Serial &); + Serial &operator=(const Serial &); + + // platform implementation + SerialImpl _impl; +}; + +} // namespace device diff --git a/platforms/common/include/px4_platform_common/SerialCommon.hpp b/platforms/common/include/px4_platform_common/SerialCommon.hpp new file mode 100644 index 000000000000..bbe9be0ad9fe --- /dev/null +++ b/platforms/common/include/px4_platform_common/SerialCommon.hpp @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * Copyright (C) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +namespace device +{ +namespace SerialConfig +{ + + +// ByteSize: number of data bits +enum class ByteSize { + FiveBits = 5, + SixBits = 6, + SevenBits = 7, + EightBits = 8, +}; + +// Parity: enable parity checking +enum class Parity { + None = 0, + Odd = 1, + Even = 2, +}; + +// StopBits: number of stop bits +enum class StopBits { + One = 1, + Two = 2 +}; + +// FlowControl: enable flow control +enum class FlowControl { + Disabled = 0, + Enabled = 1, +}; + +} // namespace SerialConfig +} // namespace device diff --git a/platforms/common/include/px4_platform_common/board_common.h b/platforms/common/include/px4_platform_common/board_common.h index b05d83271848..970c47ec9e08 100644 --- a/platforms/common/include/px4_platform_common/board_common.h +++ b/platforms/common/include/px4_platform_common/board_common.h @@ -269,6 +269,18 @@ # define HW_VER_REV(v,r) ((uint32_t)((v) & 0xffff) << 16) | ((uint32_t)(r) & 0xffff) #endif +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) +typedef uint16_t hw_fmun_id_t; +typedef uint16_t hw_base_id_t; +// Original Signals GPIO_HW_REV_SENSE/GPIO_HW_VER_REV_DRIVE is used to ID the FMUM +// Original Signals GPIO_HW_VER_SENSE/GPIO_HW_VER_REV_DRIVE is used to ID the BASE +# define BOARD_HAS_VERSIONING 1 +# define HW_FMUM_ID(rev) ((hw_fmun_id_t)(rev) & 0xffff) +# define HW_BASE_ID(ver) ((hw_base_id_t)(ver) & 0xffff) +# define GET_HW_FMUM_ID() (HW_FMUM_ID(board_get_hw_revision())) +# define GET_HW_BASE_ID() (HW_BASE_ID(board_get_hw_version())) +#endif + #define HW_INFO_REV_DIGITS 3 #define HW_INFO_VER_DIGITS 3 @@ -458,6 +470,15 @@ static inline bool board_rc_singlewire(const char *device) { return false; } * A board may define RC_SERIAL_SWAP_RXTX, so that RC_SERIAL_PORT is configured * as UART with RX/TX swapped. * + * It can optionaly define RC_SERIAL_SWAP_USING_SINGLEWIRE If the board is wired + * with TX to the input (Swapped) and the SoC does not support U[S]ART level + * HW swapping, then use onewire to do the swap if and only if: + * + * RC_SERIAL_SWAP_USING_SINGLEWIRE is defined + * RC_SERIAL_SWAP_RXTX is defined + * TIOCSSWAP is defined and retuns !OK + * TIOCSSINGLEWIRE is defined + * * Input Parameters: * device: serial device, e.g. "/dev/ttyS0" * @@ -659,20 +680,51 @@ bool board_booted_by_px4(void); ************************************************************************************/ typedef enum { - PX4_MFT_PX4IO = 0, - PX4_MFT_USB = 1, - PX4_MFT_CAN2 = 2, - PX4_MFT_CAN3 = 3, + PX4_MFT_PX4IO = 0, + PX4_MFT_USB = 1, + PX4_MFT_CAN2 = 2, + PX4_MFT_CAN3 = 3, + PX4_MFT_PM2 = 4, + PX4_MFT_ETHERNET = 5, + PX4_MFT_T1_ETH = 6, + PX4_MFT_T100_ETH = 7, + PX4_MFT_T1000_ETH = 8, } px4_hw_mft_item_id_t; +typedef int (*system_query_func_t)(const char *sub, const char *val, void *out); + +#define PX4_MFT_MFT_TYPES { \ + PX4_MFT_PX4IO, \ + PX4_MFT_USB, \ + PX4_MFT_CAN2, \ + PX4_MFT_CAN3, \ + PX4_MFT_PM2, \ + PX4_MFT_ETHERNET, \ + PX4_MFT_T1_ETH, \ + PX4_MFT_T100_ETH, \ + PX4_MFT_T1000_ETH } + +#define PX4_MFT_MFT_STR_TYPES { \ + "MFT_PX4IO", \ + "MFT_USB", \ + "MFT_CAN2", \ + "MFT_CAN3", \ + "MFT_PM2", \ + "MFT_ETHERNET", \ + "MFT_T1_ETH", \ + "MFT_T100_ETH", \ + "MFT_T1000_ETH", \ + "MFT_T1000_ETH"} + typedef enum { - px4_hw_con_unknown = 0, - px4_hw_con_onboard = 1, + px4_hw_con_unknown = 0, + px4_hw_con_onboard = 1, px4_hw_con_connector = 3, } px4_hw_connection_t; typedef struct { + unsigned int id: 16; /* The id px4_hw_mft_item_id_t */ unsigned int present: 1; /* 1 if this board have this item */ unsigned int mandatory: 1; /* 1 if this item has to be present and working */ unsigned int connection: 2; /* See px4_hw_connection_t */ @@ -684,7 +736,7 @@ typedef const px4_hw_mft_item_t *px4_hw_mft_item; #if defined(BOARD_HAS_VERSIONING) __EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id); - +__EXPORT int system_query_manifest(const char *sub, const char *val, void *out); # define PX4_MFT_HW_SUPPORTED(ID) (board_query_manifest((ID))->present) # define PX4_MFT_HW_REQUIRED(ID) (board_query_manifest((ID))->mandatory) # define PX4_MFT_HW_IS_ONBOARD(ID) (board_query_manifest((ID))->connection == px4_hw_con_onboard) @@ -757,6 +809,26 @@ __EXPORT const char *board_get_hw_type_name(void); #define board_get_hw_type_name() "" #endif +/************************************************************************************ + * Name: board_get_hw_base_type_name + * + * 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 may be a 0 length string "" + * + ************************************************************************************/ + +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) +__EXPORT const char *board_get_hw_base_type_name(void); +#else +#define board_get_hw_base_type_name() "" +#endif + /************************************************************************************ * Name: board_get_hw_version * diff --git a/platforms/common/include/px4_platform_common/shutdown.h b/platforms/common/include/px4_platform_common/shutdown.h index f4bd2e9cf6b3..3e306f3a1795 100644 --- a/platforms/common/include/px4_platform_common/shutdown.h +++ b/platforms/common/include/px4_platform_common/shutdown.h @@ -70,6 +70,12 @@ __EXPORT int px4_register_shutdown_hook(shutdown_hook_t hook); */ __EXPORT int px4_unregister_shutdown_hook(shutdown_hook_t hook); +/** Types of reboot requests for PX4 */ +typedef enum { + REBOOT_REQUEST = 0, ///< Normal reboot + REBOOT_TO_BOOTLOADER = 1, ///< Reboot to PX4 bootloader + REBOOT_TO_ISP = 2, ///< Reboot to ISP bootloader +} reboot_request_t; /** * Request the system to reboot. @@ -83,7 +89,7 @@ __EXPORT int px4_unregister_shutdown_hook(shutdown_hook_t hook); * @return 0 on success, <0 on error */ #if defined(CONFIG_BOARDCTL_RESET) -__EXPORT int px4_reboot_request(bool to_bootloader = false, uint32_t delay_us = 0); +__EXPORT int px4_reboot_request(reboot_request_t request = REBOOT_REQUEST, uint32_t delay_us = 0); #endif // CONFIG_BOARDCTL_RESET diff --git a/platforms/common/include/px4_platform_common/spi.h b/platforms/common/include/px4_platform_common/spi.h index 66644098228f..a2e4682b98cd 100644 --- a/platforms/common/include/px4_platform_common/spi.h +++ b/platforms/common/include/px4_platform_common/spi.h @@ -73,7 +73,11 @@ struct px4_spi_bus_t { struct px4_spi_bus_all_hw_t { px4_spi_bus_t buses[SPI_BUS_MAX_BUS_ITEMS]; - int board_hw_version_revision{-1}; ///< 0=default, >0 for a specific revision (see board_get_hw_version & board_get_hw_revision), -1=unused +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) + hw_fmun_id_t board_hw_fmun_id {USHRT_MAX}; +#else + int board_hw_version_revision {-1}; ///< 0=default, >0 for a specific revision (see board_get_hw_version & board_get_hw_revision), -1=unused +#endif }; #if BOARD_NUM_SPI_CFG_HW_VERSIONS > 1 diff --git a/platforms/common/include/px4_platform_common/time.h b/platforms/common/include/px4_platform_common/time.h index 1b4d8a27560c..d61e23c75cfb 100644 --- a/platforms/common/include/px4_platform_common/time.h +++ b/platforms/common/include/px4_platform_common/time.h @@ -13,11 +13,21 @@ __END_DECLS #define px4_clock_gettime system_clock_gettime #endif -#if defined(ENABLE_LOCKSTEP_SCHEDULER) +#if defined(ENABLE_LOCKSTEP_SCHEDULER) || defined(__PX4_QURT) __BEGIN_DECLS __EXPORT int px4_clock_settime(clockid_t clk_id, const struct timespec *tp); +__END_DECLS + +#else + +#define px4_clock_settime system_clock_settime +#endif + +#if defined(ENABLE_LOCKSTEP_SCHEDULER) + +__BEGIN_DECLS __EXPORT int px4_usleep(useconds_t usec); __EXPORT unsigned int px4_sleep(unsigned int seconds); __EXPORT int px4_pthread_cond_timedwait(pthread_cond_t *cond, @@ -27,7 +37,6 @@ __END_DECLS #else -#define px4_clock_settime system_clock_settime #define px4_usleep system_usleep #define px4_sleep system_sleep #define px4_pthread_cond_timedwait system_pthread_cond_timedwait diff --git a/platforms/common/pab_manifest.c b/platforms/common/pab_manifest.c new file mode 100644 index 000000000000..979bb5ecbc9b --- /dev/null +++ b/platforms/common/pab_manifest.c @@ -0,0 +1,446 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file pab_manifest.c + * + * This module supplies the interface to the manifest of hardware that is + * optional and dependent on the HW_BASE_ID + * + * 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 + +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) + +#include +#include +#include + +#include "systemlib/px4_macros.h" + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +typedef struct { + hw_base_id_t hw_base_id; /* The ID of the Base */ + 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 base board configuration +// The ids of those components is given by the enum (px4_hw_mft_item_id_t) +// declared in board_common.h + +// BASE ID 0 Auterion vXx base board +static const px4_hw_mft_item_t base_configuration_0[] = { + { + .id = PX4_MFT_PX4IO, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_USB, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_CAN2, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_CAN3, + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_unknown, + }, + { + .id = PX4_MFT_PM2, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_ETHERNET, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_connector, + }, + { + .id = PX4_MFT_T100_ETH, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_connector, + }, +}; + +// BASE ID 1 vXx base without px4io +static const px4_hw_mft_item_t base_configuration_1[] = { + { + .id = PX4_MFT_PX4IO, + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_unknown, + }, + { + .id = PX4_MFT_USB, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_CAN2, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_CAN3, + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_unknown, + }, + { + .id = PX4_MFT_PM2, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_ETHERNET, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_connector, + }, + { + .id = PX4_MFT_T100_ETH, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_connector, + }, +}; + +// BASE ID 2 Modal AI Alaised to ID 0 + +// BASE ID 3 NXP T1 PHY +static const px4_hw_mft_item_t base_configuration_3[] = { + { + .id = PX4_MFT_PX4IO, + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_unknown, + }, + { + .id = PX4_MFT_USB, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_CAN2, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_CAN3, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_PM2, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_ETHERNET, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_connector, + }, + { + .id = PX4_MFT_T1_ETH, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_connector, + }, +}; + +// BASE ID 4 HB CM4 Alaised to ID 0 + +// BASE ID 5 HB min +static const px4_hw_mft_item_t base_configuration_5[] = { + { + .id = PX4_MFT_PX4IO, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_USB, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_CAN2, + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_unknown, + }, + { + .id = PX4_MFT_CAN3, + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_unknown, + }, + { + .id = PX4_MFT_ETHERNET, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_connector, + }, + { + .id = PX4_MFT_T100_ETH, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_connector, + }, +}; + +// BASE ID 6 Not allocated +// BASE ID 7 Read from EEPROM +// BASE ID 8 Skynode QS with USB - Alaised to ID 0 + +// BASE ID 9 Auterion Skynode base RC9 & older (no usb +static const px4_hw_mft_item_t base_configuration_9[] = { + { + .id = PX4_MFT_PX4IO, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_USB, + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_unknown, + }, + { + .id = PX4_MFT_CAN2, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_CAN3, + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_unknown, + }, + { + .id = PX4_MFT_PM2, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_ETHERNET, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_connector, + }, + { + .id = PX4_MFT_T100_ETH, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, +}; + +// BASE ID 10 Skynode QS no USB Alaised to ID 9 +// BASE ID 16 Auterion Skynode RC10, RC11, RC12, RC13 Alaised to ID 0 + +// BASE ID 17 Auterion Skynode RC13 with many parts removed +static const px4_hw_mft_item_t base_configuration_17[] = { + { + .id = PX4_MFT_PX4IO, + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_USB, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_unknown, + }, + { + .id = PX4_MFT_CAN2, + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_CAN3, + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_unknown, + }, + { + .id = PX4_MFT_PM2, + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_ETHERNET, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_connector, + }, + { + .id = PX4_MFT_T100_ETH, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, +}; + + +static px4_hw_mft_list_entry_t mft_lists[] = { +// ver_rev + {HW_BASE_ID(0), base_configuration_0, arraySize(base_configuration_0)}, // std Base with PX4IO + {HW_BASE_ID(1), base_configuration_1, arraySize(base_configuration_1)}, // std Base No PX4IO + {HW_BASE_ID(2), base_configuration_0, arraySize(base_configuration_0)}, // CUAV Base + {HW_BASE_ID(3), base_configuration_3, arraySize(base_configuration_3)}, // NXP T1 PHY + {HW_BASE_ID(4), base_configuration_0, arraySize(base_configuration_0)}, // HB CM4 base + {HW_BASE_ID(5), base_configuration_5, arraySize(base_configuration_5)}, // HB Mini + {HW_BASE_ID(8), base_configuration_0, arraySize(base_configuration_0)}, // Auterion Skynode ver 8 Aliased to 0 + {HW_BASE_ID(9), base_configuration_9, arraySize(base_configuration_9)}, // Auterion Skynode ver 9 + {HW_BASE_ID(10), base_configuration_9, arraySize(base_configuration_9)}, // Auterion Skynode ver 10 + {HW_BASE_ID(16), base_configuration_0, arraySize(base_configuration_0)}, // Auterion Skynode ver 16 + {HW_BASE_ID(17), base_configuration_17, arraySize(base_configuration_17)}, // Auterion Skynode ver 17 +}; + +/************************************************************************************ + * Name: base_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) { + hw_base_id_t hw_base_id = GET_HW_BASE_ID(); + + for (unsigned i = 0; i < arraySize(mft_lists); i++) { + if (mft_lists[i].hw_base_id == hw_base_id) { + boards_manifest = &mft_lists[i]; + break; + } + } + + if (boards_manifest == px4_hw_mft_list_uninitialized) { + syslog(LOG_ERR, "[boot] Board %04" PRIx16 " is not supported!\n", hw_base_id); + } + } + + px4_hw_mft_item rv = &device_unsupported; + + if (boards_manifest != px4_hw_mft_list_uninitialized) + for (unsigned int ndx = 0; ndx < boards_manifest->entries; ndx++) { + if (boards_manifest->mft[ndx].id == id) { + rv = &boards_manifest->mft[id]; + break; + } + } + + return rv; +} + +__EXPORT int system_query_manifest(const char *sub, const char *val, void *out) +{ + static const char *keys[] = PX4_MFT_MFT_STR_TYPES; + static const px4_hw_mft_item_id_t item_ids[] = PX4_MFT_MFT_TYPES; + px4_hw_mft_item rv = &device_unsupported; + int id = -1; + int intval = atoi(val); + + for (unsigned int k = 0; k < arraySize(keys); k++) { + if (!strcmp(keys[k], sub)) { + id = item_ids[k]; + break; + } + } + + if (id != -1) { + // In case we have to filter when a FMUM is mounted to a BASE + // For now just use the board + rv = board_query_manifest(id); + return rv->present == intval ? OK : -ENXIO; + } + + return -ENOENT; +} +#endif diff --git a/platforms/common/px4_log.cpp b/platforms/common/px4_log.cpp index a3d9b02338aa..73a3671c8982 100644 --- a/platforms/common/px4_log.cpp +++ b/platforms/common/px4_log.cpp @@ -74,7 +74,7 @@ void px4_log_initialize(void) log_message.severity = 6; // info strcpy((char *)log_message.text, "initialized uORB logging"); log_message.timestamp = hrt_absolute_time(); - orb_log_message_pub = orb_advertise_queue(ORB_ID(log_message), &log_message, log_message_s::ORB_QUEUE_LENGTH); + orb_log_message_pub = orb_advertise(ORB_ID(log_message), &log_message); } __EXPORT void px4_log_modulename(int level, const char *module_name, const char *fmt, ...) diff --git a/platforms/common/shutdown.cpp b/platforms/common/shutdown.cpp index dcbd733ceb29..878d99c62efd 100644 --- a/platforms/common/shutdown.cpp +++ b/platforms/common/shutdown.cpp @@ -108,6 +108,7 @@ static uint16_t shutdown_counter = 0; ///< count how many times the shutdown wor #define SHUTDOWN_ARG_IN_PROGRESS (1<<0) #define SHUTDOWN_ARG_REBOOT (1<<1) #define SHUTDOWN_ARG_TO_BOOTLOADER (1<<2) +#define SHUTDOWN_ARG_TO_ISP (1<<3) static uint8_t shutdown_args = 0; static constexpr int max_shutdown_hooks = 1; @@ -175,7 +176,17 @@ static void shutdown_worker(void *arg) if (shutdown_args & SHUTDOWN_ARG_REBOOT) { #if defined(CONFIG_BOARDCTL_RESET) PX4_INFO_RAW("Reboot NOW."); - boardctl(BOARDIOC_RESET, (shutdown_args & SHUTDOWN_ARG_TO_BOOTLOADER) ? 1 : 0); + + if (shutdown_args & SHUTDOWN_ARG_TO_BOOTLOADER) { + boardctl(BOARDIOC_RESET, (uintptr_t)REBOOT_TO_BOOTLOADER); + + } else if (shutdown_args & SHUTDOWN_ARG_TO_ISP) { + boardctl(BOARDIOC_RESET, (uintptr_t)REBOOT_TO_ISP); + + } else { + boardctl(BOARDIOC_RESET, (uintptr_t)REBOOT_REQUEST); + } + #else PX4_PANIC("board reset not available"); #endif @@ -206,7 +217,7 @@ static void shutdown_worker(void *arg) } #if defined(CONFIG_BOARDCTL_RESET) -int px4_reboot_request(bool to_bootloader, uint32_t delay_us) +int px4_reboot_request(reboot_request_t request, uint32_t delay_us) { pthread_mutex_lock(&shutdown_mutex); @@ -217,8 +228,11 @@ int px4_reboot_request(bool to_bootloader, uint32_t delay_us) shutdown_args |= SHUTDOWN_ARG_REBOOT; - if (to_bootloader) { + if (request == REBOOT_TO_BOOTLOADER) { shutdown_args |= SHUTDOWN_ARG_TO_BOOTLOADER; + + } else if (request == REBOOT_TO_ISP) { + shutdown_args |= SHUTDOWN_ARG_TO_ISP; } shutdown_time_us = hrt_absolute_time(); diff --git a/platforms/common/spi.cpp b/platforms/common/spi.cpp index adda7b4b5795..963372bdbe0f 100644 --- a/platforms/common/spi.cpp +++ b/platforms/common/spi.cpp @@ -44,12 +44,26 @@ #if BOARD_NUM_SPI_CFG_HW_VERSIONS > 1 void px4_set_spi_buses_from_hw_version() { -#if defined(BOARD_HAS_SIMPLE_HW_VERSIONING) +# if defined(BOARD_HAS_HW_SPLIT_VERSIONING) + int hw_fmun_id = GET_HW_FMUM_ID(); + + for (int i = 0; i < BOARD_NUM_SPI_CFG_HW_VERSIONS; ++i) { + if (!px4_spi_buses && px4_spi_buses_all_hw[i].board_hw_fmun_id == 0) { + px4_spi_buses = px4_spi_buses_all_hw[i].buses; + } + + if (px4_spi_buses_all_hw[i].board_hw_fmun_id == hw_fmun_id) { + px4_spi_buses = px4_spi_buses_all_hw[i].buses; + } + } + +# else + +# if defined(BOARD_HAS_SIMPLE_HW_VERSIONING) int hw_version_revision = board_get_hw_version(); -#else +# else int hw_version_revision = HW_VER_REV(board_get_hw_version(), board_get_hw_revision()); -#endif - +# endif for (int i = 0; i < BOARD_NUM_SPI_CFG_HW_VERSIONS; ++i) { if (!px4_spi_buses && px4_spi_buses_all_hw[i].board_hw_version_revision == 0) { @@ -61,6 +75,8 @@ void px4_set_spi_buses_from_hw_version() } } +# endif + if (!px4_spi_buses) { // fallback px4_spi_buses = px4_spi_buses_all_hw[0].buses; } diff --git a/platforms/common/uORB/Publication.hpp b/platforms/common/uORB/Publication.hpp index 7547e3b7faf3..a0a7cc20cf71 100644 --- a/platforms/common/uORB/Publication.hpp +++ b/platforms/common/uORB/Publication.hpp @@ -48,24 +48,6 @@ namespace uORB { -template class DefaultQueueSize -{ -private: - template - static constexpr uint8_t get_queue_size(decltype(T::ORB_QUEUE_LENGTH) *) - { - return T::ORB_QUEUE_LENGTH; - } - - template static constexpr uint8_t get_queue_size(...) - { - return 1; - } - -public: - static constexpr unsigned value = get_queue_size(nullptr); -}; - class PublicationBase { public: @@ -97,7 +79,7 @@ class PublicationBase /** * uORB publication wrapper class */ -template::value> +template class Publication : public PublicationBase { public: @@ -113,7 +95,7 @@ class Publication : public PublicationBase bool advertise() { if (!advertised()) { - _handle = orb_advertise_queue(get_topic(), nullptr, ORB_QSIZE); + _handle = orb_advertise(get_topic(), nullptr); } return advertised(); diff --git a/platforms/common/uORB/PublicationMulti.hpp b/platforms/common/uORB/PublicationMulti.hpp index bc275940b604..6e8863b17d12 100644 --- a/platforms/common/uORB/PublicationMulti.hpp +++ b/platforms/common/uORB/PublicationMulti.hpp @@ -51,7 +51,7 @@ namespace uORB /** * Base publication multi wrapper class */ -template::value> +template class PublicationMulti : public PublicationBase { public: @@ -73,7 +73,7 @@ class PublicationMulti : public PublicationBase { if (!advertised()) { int instance = 0; - _handle = orb_advertise_multi_queue(get_topic(), nullptr, &instance, QSIZE); + _handle = orb_advertise_multi(get_topic(), nullptr, &instance); } return advertised(); diff --git a/platforms/common/uORB/SubscriptionBlocking.hpp b/platforms/common/uORB/SubscriptionBlocking.hpp index 7ec29e329b6e..5c5580d855cb 100644 --- a/platforms/common/uORB/SubscriptionBlocking.hpp +++ b/platforms/common/uORB/SubscriptionBlocking.hpp @@ -56,31 +56,6 @@ class SubscriptionBlocking : public SubscriptionCallback SubscriptionBlocking(const orb_metadata *meta, uint32_t interval_us = 0, uint8_t instance = 0) : SubscriptionCallback(meta, interval_us, instance) { - // pthread_mutexattr_init - pthread_mutexattr_t attr; - int ret_attr_init = pthread_mutexattr_init(&attr); - - if (ret_attr_init != 0) { - PX4_ERR("pthread_mutexattr_init failed, status=%d", ret_attr_init); - } - -#if defined(PTHREAD_PRIO_NONE) - // pthread_mutexattr_settype - // PTHREAD_PRIO_NONE not available on cygwin - int ret_mutexattr_settype = pthread_mutexattr_settype(&attr, PTHREAD_PRIO_NONE); - - if (ret_mutexattr_settype != 0) { - PX4_ERR("pthread_mutexattr_settype failed, status=%d", ret_mutexattr_settype); - } - -#endif // PTHREAD_PRIO_NONE - - // pthread_mutex_init - int ret_mutex_init = pthread_mutex_init(&_mutex, &attr); - - if (ret_mutex_init != 0) { - PX4_ERR("pthread_mutex_init failed, status=%d", ret_mutex_init); - } } virtual ~SubscriptionBlocking() diff --git a/platforms/common/uORB/uORB.cpp b/platforms/common/uORB/uORB.cpp index c91ac6bccec3..a68f3c5dc83c 100644 --- a/platforms/common/uORB/uORB.cpp +++ b/platforms/common/uORB/uORB.cpp @@ -118,22 +118,11 @@ orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) return uORB::Manager::get_instance()->orb_advertise(meta, data); } -orb_advert_t orb_advertise_queue(const struct orb_metadata *meta, const void *data, unsigned int queue_size) -{ - return uORB::Manager::get_instance()->orb_advertise(meta, data, queue_size); -} - orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance) { return uORB::Manager::get_instance()->orb_advertise_multi(meta, data, instance); } -orb_advert_t orb_advertise_multi_queue(const struct orb_metadata *meta, const void *data, int *instance, - unsigned int queue_size) -{ - return uORB::Manager::get_instance()->orb_advertise_multi(meta, data, instance, queue_size); -} - int orb_unadvertise(orb_advert_t handle) { return uORB::Manager::get_instance()->orb_unadvertise(handle); @@ -227,6 +216,14 @@ const char *orb_get_c_type(unsigned char short_type) return nullptr; } +uint8_t orb_get_queue_size(const struct orb_metadata *meta) +{ + if (meta) { + return meta->o_queue; + } + + return 0; +} void orb_print_message_internal(const orb_metadata *meta, const void *data, bool print_topic_name) { diff --git a/platforms/common/uORB/uORB.h b/platforms/common/uORB/uORB.h index fc06a2da3cda..a8964b4d27cd 100644 --- a/platforms/common/uORB/uORB.h +++ b/platforms/common/uORB/uORB.h @@ -53,6 +53,8 @@ struct orb_metadata { const uint16_t o_size_no_padding; /**< object size w/o padding at the end (for logger) */ uint32_t message_hash; /**< Hash over all fields for message compatibility checks */ orb_id_size_t o_id; /**< ORB_ID enum */ + uint8_t o_queue; /**< queue size */ + }; typedef const struct orb_metadata *orb_id_t; @@ -102,14 +104,16 @@ typedef const struct orb_metadata *orb_id_t; * @param _size_no_padding Struct size w/o padding at the end * @param _message_hash 32 bit message hash over all fields * @param _orb_id_enum ORB ID enum e.g.: ORB_ID::vehicle_status + * @param _queue_size Queue size from topic definition */ -#define ORB_DEFINE(_name, _struct, _size_no_padding, _message_hash, _orb_id_enum) \ - const struct orb_metadata __orb_##_name = { \ - #_name, \ - sizeof(_struct), \ - _size_no_padding, \ - _message_hash, \ - _orb_id_enum \ +#define ORB_DEFINE(_name, _struct, _size_no_padding, _message_hash, _orb_id_enum, _queue_size) \ + const struct orb_metadata __orb_##_name = { \ + #_name, \ + sizeof(_struct), \ + _size_no_padding, \ + _message_hash, \ + _orb_id_enum, \ + _queue_size \ }; struct hack __BEGIN_DECLS @@ -135,23 +139,11 @@ typedef void *orb_advert_t; */ extern orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) __EXPORT; -/** - * @see uORB::Manager::orb_advertise() - */ -extern orb_advert_t orb_advertise_queue(const struct orb_metadata *meta, const void *data, - unsigned int queue_size) __EXPORT; - /** * @see uORB::Manager::orb_advertise_multi() */ extern orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance) __EXPORT; -/** - * @see uORB::Manager::orb_advertise_multi() - */ -extern orb_advert_t orb_advertise_multi_queue(const struct orb_metadata *meta, const void *data, int *instance, - unsigned int queue_size) __EXPORT; - /** * @see uORB::Manager::orb_unadvertise() */ @@ -160,7 +152,7 @@ extern int orb_unadvertise(orb_advert_t handle) __EXPORT; /** * @see uORB::Manager::orb_publish() */ -extern int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) __EXPORT; +extern int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) __EXPORT; /** * Advertise as the publisher of a topic. @@ -241,6 +233,12 @@ extern int orb_get_interval(int handle, unsigned *interval) __EXPORT; */ const char *orb_get_c_type(unsigned char short_type); +/** + * Returns the queue size of a topic + * @param meta orb topic metadata + */ +extern uint8_t orb_get_queue_size(const struct orb_metadata *meta); + /** * Print a topic to console. Do not call this directly, use print_message() instead. * @param meta orb topic metadata diff --git a/platforms/common/uORB/uORBDeviceNode.cpp b/platforms/common/uORB/uORBDeviceNode.cpp index 112170de6b5b..fd4ef60e63e0 100644 --- a/platforms/common/uORB/uORBDeviceNode.cpp +++ b/platforms/common/uORB/uORBDeviceNode.cpp @@ -48,37 +48,10 @@ static uORB::SubscriptionInterval *filp_to_subscription(cdev::file_t *filp) { return static_cast(filp->f_priv); } -// round up to nearest power of two -// Such as 0 => 1, 1 => 1, 2 => 2 ,3 => 4, 10 => 16, 60 => 64, 65...255 => 128 -// Note: When the input value > 128, the output is always 128 -static inline uint8_t round_pow_of_two_8(uint8_t n) -{ - if (n == 0) { - return 1; - } - - // Avoid is already a power of 2 - uint8_t value = n - 1; - - // Fill 1 - value |= value >> 1U; - value |= value >> 2U; - value |= value >> 4U; - - // Unable to round-up, take the value of round-down - if (value == UINT8_MAX) { - value >>= 1U; - } - - return value + 1; -} - -uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path, - uint8_t queue_size) : +uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path) : CDev(strdup(path)), // success is checked in CDev::init _meta(meta), - _instance(instance), - _queue_size(round_pow_of_two_8(queue_size)) + _instance(instance) { } @@ -186,7 +159,7 @@ uORB::DeviceNode::write(cdev::file_t *filp, const char *buffer, size_t buflen) /* re-check size */ if (nullptr == _data) { - const size_t data_size = _meta->o_size * _queue_size; + const size_t data_size = _meta->o_size * _meta->o_queue; _data = (uint8_t *) px4_cache_aligned_alloc(data_size); if (_data) { @@ -217,7 +190,7 @@ uORB::DeviceNode::write(cdev::file_t *filp, const char *buffer, size_t buflen) /* wrap-around happens after ~49 days, assuming a publisher rate of 1 kHz */ unsigned generation = _generation.fetch_add(1); - memcpy(_data + (_meta->o_size * (generation % _queue_size)), buffer, _meta->o_size); + memcpy(_data + (_meta->o_size * (generation % _meta->o_queue)), buffer, _meta->o_size); // callbacks for (auto item : _callbacks) { @@ -254,13 +227,6 @@ uORB::DeviceNode::ioctl(cdev::file_t *filp, int cmd, unsigned long arg) *(uintptr_t *)arg = (uintptr_t)this; return PX4_OK; - case ORBIOCSETQUEUESIZE: { - lock(); - int ret = update_queue_size(arg); - unlock(); - return ret; - } - case ORBIOCGETINTERVAL: *(unsigned *)arg = filp_to_subscription(filp)->get_interval_us(); return PX4_OK; @@ -389,12 +355,11 @@ uORB::DeviceNode::print_statistics(int max_topic_length) const uint8_t instance = get_instance(); const int8_t sub_count = subscriber_count(); - const uint8_t queue_size = get_queue_size(); unlock(); PX4_INFO_RAW("%-*s %2i %4i %2i %4i %s\n", max_topic_length, get_meta()->o_name, (int)instance, (int)sub_count, - queue_size, get_meta()->o_size, get_devname()); + get_meta()->o_queue, get_meta()->o_size, get_devname()); return true; } @@ -447,7 +412,10 @@ int16_t uORB::DeviceNode::process_add_subscription() uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator(); if (_data != nullptr && ch != nullptr) { // _data will not be null if there is a publisher. - ch->send_message(_meta->o_name, _meta->o_size, _data); + // Only send the most recent data to initialize the remote end. + if (_data_valid) { + ch->send_message(_meta->o_name, _meta->o_size, _data + (_meta->o_size * ((_generation.load() - 1) % _meta->o_queue))); + } } return PX4_OK; @@ -483,21 +451,6 @@ int16_t uORB::DeviceNode::process_received_message(int32_t length, uint8_t *data } #endif /* CONFIG_ORB_COMMUNICATOR */ -int uORB::DeviceNode::update_queue_size(unsigned int queue_size) -{ - if (_queue_size == queue_size) { - return PX4_OK; - } - - //queue size is limited to 255 for the single reason that we use uint8 to store it - if (_data || _queue_size > queue_size || queue_size > 255) { - return PX4_ERROR; - } - - _queue_size = round_pow_of_two_8(queue_size); - return PX4_OK; -} - unsigned uORB::DeviceNode::get_initial_generation() { ATOMIC_ENTER; diff --git a/platforms/common/uORB/uORBDeviceNode.hpp b/platforms/common/uORB/uORBDeviceNode.hpp index c5c5bb22c0c8..527c5ddf2b4f 100644 --- a/platforms/common/uORB/uORBDeviceNode.hpp +++ b/platforms/common/uORB/uORBDeviceNode.hpp @@ -62,7 +62,7 @@ class UnitTest; class uORB::DeviceNode : public cdev::CDev, public IntrusiveSortedListNode { public: - DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path, uint8_t queue_size = 1); + DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path); virtual ~DeviceNode(); // no copy, assignment, move, move assignment @@ -179,15 +179,6 @@ class uORB::DeviceNode : public cdev::CDev, public IntrusiveSortedListNodeo_queue; } int8_t subscriber_count() const { return _subscriber_count; } @@ -234,7 +225,7 @@ class uORB::DeviceNode : public cdev::CDev, public IntrusiveSortedListNodeo_queue == 1) { ATOMIC_ENTER; memcpy(dst, _data, _meta->o_size); generation = _generation.load(); @@ -253,12 +244,12 @@ class uORB::DeviceNode : public cdev::CDev, public IntrusiveSortedListNodeo_queue, generation, current_generation - 1)) { // Reader is too far behind: some messages are lost - generation = current_generation - _queue_size; + generation = current_generation - _meta->o_queue; } - memcpy(dst, _data + (_meta->o_size * (generation % _queue_size)), _meta->o_size); + memcpy(dst, _data + (_meta->o_size * (generation % _meta->o_queue)), _meta->o_size); ATOMIC_LEAVE; ++generation; @@ -295,7 +286,7 @@ class uORB::DeviceNode : public cdev::CDev, public IntrusiveSortedListNode 1) { - PX4_WARN("orb_advertise_multi: failed to set queue size"); - } - /* get the advertiser handle and close the node */ orb_advert_t advertiser; - result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser); + int result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser); px4_close(fd); if (result == PX4_ERROR) { @@ -602,6 +592,22 @@ int16_t uORB::Manager::process_remote_topic(const char *topic_name) { PX4_DEBUG("entering process_remote_topic: name: %s", topic_name); + // First make sure this is a valid topic + const struct orb_metadata *const *topic_list = orb_get_topics(); + orb_id_t topic_ptr = nullptr; + + for (size_t i = 0; i < orb_topics_count(); i++) { + if (strcmp(topic_list[i]->o_name, topic_name) == 0) { + topic_ptr = topic_list[i]; + break; + } + } + + if (! topic_ptr) { + PX4_ERR("process_remote_topic meta not found for %s\n", topic_name); + return -1; + } + // Look to see if we already have a node for this topic char nodepath[orb_maxpath]; int ret = uORB::Utils::node_mkpath(nodepath, topic_name); @@ -613,7 +619,7 @@ int16_t uORB::Manager::process_remote_topic(const char *topic_name) uORB::DeviceNode *node = device_master->getDeviceNode(nodepath); if (node) { - PX4_INFO("Marking DeviceNode(%s) as advertised in process_remote_topic", topic_name); + PX4_DEBUG("Marking DeviceNode(%s) as advertised in process_remote_topic", topic_name); node->mark_as_advertised(); _remote_topics.insert(topic_name); return 0; @@ -622,27 +628,9 @@ int16_t uORB::Manager::process_remote_topic(const char *topic_name) } // We didn't find a node so we need to create it via an advertisement - const struct orb_metadata *const *topic_list = orb_get_topics(); - orb_id_t topic_ptr = nullptr; - - for (size_t i = 0; i < orb_topics_count(); i++) { - if (strcmp(topic_list[i]->o_name, topic_name) == 0) { - topic_ptr = topic_list[i]; - break; - } - } - - if (topic_ptr) { - PX4_INFO("Advertising remote topic %s", topic_name); - _remote_topics.insert(topic_name); - // Add some queue depth when advertising remote topics. These - // topics may get aggregated and thus delivered in a batch that - // requires some buffering in a queue. - orb_advertise(topic_ptr, nullptr, 5); - - } else { - PX4_INFO("process_remote_topic meta not found for %s\n", topic_name); - } + PX4_DEBUG("Advertising remote topic %s", topic_name); + _remote_topics.insert(topic_name); + orb_advertise(topic_ptr, nullptr); return 0; } @@ -663,8 +651,11 @@ int16_t uORB::Manager::process_add_subscription(const char *messageName) PX4_DEBUG("DeviceNode(%s) not created yet", messageName); } else { - // node is present. - node->process_add_subscription(); + // node is present. But don't send any data to it if it + // is a node advertised by the remote side + if (_remote_topics.find(messageName) == false) { + node->process_add_subscription(); + } } } else { diff --git a/platforms/common/uORB/uORBManager.hpp b/platforms/common/uORB/uORBManager.hpp index 1c2870b1eecd..d5202b9236af 100644 --- a/platforms/common/uORB/uORBManager.hpp +++ b/platforms/common/uORB/uORBManager.hpp @@ -215,17 +215,15 @@ class uORB::Manager * @param data A pointer to the initial data to be published. * For topics updated by interrupt handlers, the advertisement * must be performed from non-interrupt context. - * @param queue_size Maximum number of buffered elements. If this is 1, no queuing is - * used. * @return nullptr on error, otherwise returns an object pointer * that can be used to publish to the topic. * If the topic in question is not known (due to an * ORB_DEFINE with no corresponding ORB_DECLARE) * this function will return nullptr and set errno to ENOENT. */ - orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data, unsigned int queue_size = 1) + orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data = nullptr) { - return orb_advertise_multi(meta, data, nullptr, queue_size); + return orb_advertise_multi(meta, data, nullptr); } /** @@ -250,16 +248,13 @@ class uORB::Manager * @param instance Pointer to an integer which will yield the instance ID (0-based) * of the publication. This is an output parameter and will be set to the newly * created instance, ie. 0 for the first advertiser, 1 for the next and so on. - * @param queue_size Maximum number of buffered elements. If this is 1, no queuing is - * used. * @return nullptr on error, otherwise returns a handle * that can be used to publish to the topic. * If the topic in question is not known (due to an * ORB_DEFINE with no corresponding ORB_DECLARE) * this function will return nullptr and set errno to ENOENT. */ - orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, - unsigned int queue_size = 1); + orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance); /** * Unadvertise a topic. diff --git a/platforms/common/uORB/uORBManagerUsr.cpp b/platforms/common/uORB/uORBManagerUsr.cpp index af00dbb9ba58..e1a413e4c685 100644 --- a/platforms/common/uORB/uORBManagerUsr.cpp +++ b/platforms/common/uORB/uORBManagerUsr.cpp @@ -89,8 +89,7 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance) return data.ret; } -orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, - unsigned int queue_size) +orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance) { /* open the node as an advertiser */ int fd = node_open(meta, true, instance); @@ -100,19 +99,10 @@ orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, return nullptr; } - /* Set the queue size. This must be done before the first publication; thus it fails if - * this is not the first advertiser. - */ - int result = px4_ioctl(fd, ORBIOCSETQUEUESIZE, (unsigned long)queue_size); - - if (result < 0 && queue_size > 1) { - PX4_WARN("orb_advertise_multi: failed to set queue size"); - } - /* get the advertiser handle and close the node */ orb_advert_t advertiser; - result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser); + int result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser); px4_close(fd); if (result == PX4_ERROR) { diff --git a/platforms/common/uORB/uORB_tests/uORBTest_UnitTest.cpp b/platforms/common/uORB/uORB_tests/uORBTest_UnitTest.cpp index f8631fa04de0..b40b60ad4760 100644 --- a/platforms/common/uORB/uORB_tests/uORBTest_UnitTest.cpp +++ b/platforms/common/uORB/uORB_tests/uORBTest_UnitTest.cpp @@ -574,8 +574,8 @@ int uORBTest::UnitTest::test_wrap_around() bool updated{false}; // Advertise but not publish topics, only generate device_node, which is convenient for modifying DeviceNode::_generation - const int queue_size = 16; - ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_wrap_around), nullptr, queue_size); + const int queue_size = orb_get_queue_size(ORB_ID(orb_test_medium_wrap_around)); + ptopic = orb_advertise(ORB_ID(orb_test_medium_wrap_around), nullptr); if (ptopic == nullptr) { return test_fail("advertise failed: %d", errno); @@ -828,9 +828,9 @@ int uORBTest::UnitTest::test_queue() return test_fail("subscribe failed: %d", errno); } - const int queue_size = 16; + const int queue_size = orb_get_queue_size(ORB_ID(orb_test_medium_queue)); orb_test_medium_s t{}; - ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_queue), &t, queue_size); + ptopic = orb_advertise(ORB_ID(orb_test_medium_queue), &t); if (ptopic == nullptr) { return test_fail("advertise failed: %d", errno); @@ -935,9 +935,9 @@ int uORBTest::UnitTest::pub_test_queue_main() { orb_test_medium_s t{}; orb_advert_t ptopic{nullptr}; - const int queue_size = 50; + const int queue_size = orb_get_queue_size(ORB_ID(orb_test_medium_queue_poll)); - if ((ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_queue_poll), &t, queue_size)) == nullptr) { + if ((ptopic = orb_advertise(ORB_ID(orb_test_medium_queue_poll), &t)) == nullptr) { _thread_should_exit = true; return test_fail("advertise failed: %d", errno); } diff --git a/platforms/nuttx/NuttX/apps b/platforms/nuttx/NuttX/apps index 616f7024a479..e37940d8535f 160000 --- a/platforms/nuttx/NuttX/apps +++ b/platforms/nuttx/NuttX/apps @@ -1 +1 @@ -Subproject commit 616f7024a479bf209eadce133bba5dc8820a7f99 +Subproject commit e37940d8535f603a16b8f6f21c21edaf584218aa diff --git a/platforms/nuttx/NuttX/nuttx b/platforms/nuttx/NuttX/nuttx index 1e3aaefc60a1..693b9e782535 160000 --- a/platforms/nuttx/NuttX/nuttx +++ b/platforms/nuttx/NuttX/nuttx @@ -1 +1 @@ -Subproject commit 1e3aaefc60a18838dc696e8f0f335d8c989f35cc +Subproject commit 693b9e782535f12e6a4ab657c7a0c3bd92b45fb1 diff --git a/platforms/nuttx/src/bootloader/nxp/imxrt_common/main.c b/platforms/nuttx/src/bootloader/nxp/imxrt_common/main.c index 280bc1ad5b0f..1197427a942c 100644 --- a/platforms/nuttx/src/bootloader/nxp/imxrt_common/main.c +++ b/platforms/nuttx/src/bootloader/nxp/imxrt_common/main.c @@ -7,8 +7,8 @@ #include #include "hw_config.h" -#include "imxrt_flexspi_nor_flash.h" -#include "imxrt_romapi.h" +#include +#include #include #include #include diff --git a/platforms/nuttx/src/px4/common/SerialImpl.cpp b/platforms/nuttx/src/px4/common/SerialImpl.cpp new file mode 100644 index 000000000000..7490dd604cc4 --- /dev/null +++ b/platforms/nuttx/src/px4/common/SerialImpl.cpp @@ -0,0 +1,394 @@ +/**************************************************************************** + * + * Copyright (C) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include // strncpy +#include +#include +#include +#include +#include +#include + +#define MODULE_NAME "SerialImpl" + +namespace device +{ + +SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits, + FlowControl flowcontrol) : + _baudrate(baudrate), + _bytesize(bytesize), + _parity(parity), + _stopbits(stopbits), + _flowcontrol(flowcontrol) +{ + if (port) { + strncpy(_port, port, sizeof(_port) - 1); + _port[sizeof(_port) - 1] = '\0'; + + } else { + _port[0] = 0; + } +} + +SerialImpl::~SerialImpl() +{ + if (isOpen()) { + close(); + } +} + +bool SerialImpl::validateBaudrate(uint32_t baudrate) +{ + return ((baudrate == 9600) || + (baudrate == 19200) || + (baudrate == 38400) || + (baudrate == 57600) || + (baudrate == 115200) || + (baudrate == 230400) || + (baudrate == 460800) || + (baudrate == 921600)); +} + +bool SerialImpl::configure() +{ + /* process baud rate */ + int speed; + + if (! validateBaudrate(_baudrate)) { + PX4_ERR("ERR: unknown baudrate: %lu", _baudrate); + return false; + } + + switch (_baudrate) { + case 9600: speed = B9600; break; + + case 19200: speed = B19200; break; + + case 38400: speed = B38400; break; + + case 57600: speed = B57600; break; + + case 115200: speed = B115200; break; + + case 230400: speed = B230400; break; + +#ifndef B460800 +#define B460800 460800 +#endif + + case 460800: speed = B460800; break; + +#ifndef B921600 +#define B921600 921600 +#endif + + case 921600: speed = B921600; break; + + default: + PX4_ERR("ERR: unknown baudrate: %lu", _baudrate); + return false; + } + + struct termios uart_config; + + int termios_state; + + /* fill the struct for the new configuration */ + if ((termios_state = tcgetattr(_serial_fd, &uart_config)) < 0) { + PX4_ERR("ERR: %d (tcgetattr)", termios_state); + return false; + } + + /* properly configure the terminal (see also https://en.wikibooks.org/wiki/Serial_Programming/termios ) */ + + // + // Input flags - Turn off input processing + // + // convert break to null byte, no CR to NL translation, + // no NL to CR translation, don't mark parity errors or breaks + // no input parity check, don't strip high bit off, + // no XON/XOFF software flow control + // + uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | + INLCR | PARMRK | INPCK | ISTRIP | IXON); + + // + // Output flags - Turn off output processing + // + // no CR to NL translation, no NL to CR-NL translation, + // no NL to CR translation, no column 0 CR suppression, + // no Ctrl-D suppression, no fill characters, no case mapping, + // no local output processing + // + // config.c_oflag &= ~(OCRNL | ONLCR | ONLRET | + // ONOCR | ONOEOT| OFILL | OLCUC | OPOST); + uart_config.c_oflag = 0; + + // + // No line processing + // + // echo off, echo newline off, canonical mode off, + // extended input processing off, signal chars off + // + uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG); + + /* no parity, one stop bit, disable flow control */ + uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS); + + /* set baud rate */ + if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { + PX4_ERR("ERR: %d (cfsetispeed)", termios_state); + return false; + } + + if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { + PX4_ERR("ERR: %d (cfsetospeed)", termios_state); + return false; + } + + if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) { + PX4_ERR("ERR: %d (tcsetattr)", termios_state); + return false; + } + + return true; +} + +bool SerialImpl::open() +{ + if (isOpen()) { + return true; + } + + // Open the serial port + int serial_fd = ::open(_port, O_RDWR | O_NOCTTY); + + if (serial_fd < 0) { + PX4_ERR("failed to open %s err: %d", _port, errno); + return false; + } + + _serial_fd = serial_fd; + + // Configure the serial port + if (! configure()) { + PX4_ERR("failed to configure %s err: %d", _port, errno); + return false; + } + + _open = true; + + return _open; +} + +bool SerialImpl::isOpen() const +{ + return _open; +} + +bool SerialImpl::close() +{ + + if (_serial_fd >= 0) { + ::close(_serial_fd); + } + + _serial_fd = -1; + _open = false; + + return true; +} + +ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size) +{ + if (!_open) { + PX4_ERR("Cannot read from serial device until it has been opened"); + return -1; + } + + int ret = ::read(_serial_fd, buffer, buffer_size); + + if (ret < 0) { + PX4_DEBUG("%s read error %d", _port, ret); + } + + return ret; +} + +ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us) +{ + if (!_open) { + PX4_ERR("Cannot readAtLeast from serial device until it has been opened"); + return -1; + } + + if (buffer_size < character_count) { + PX4_ERR("%s: Buffer not big enough to hold desired amount of read data", __FUNCTION__); + return -1; + } + + const hrt_abstime start_time_us = hrt_absolute_time(); + int total_bytes_read = 0; + + while ((total_bytes_read < (int) character_count) && (hrt_elapsed_time(&start_time_us) < timeout_us)) { + // Poll for incoming UART data. + pollfd fds[1]; + fds[0].fd = _serial_fd; + fds[0].events = POLLIN; + + hrt_abstime remaining_time = timeout_us - hrt_elapsed_time(&start_time_us); + + if (remaining_time <= 0) { break; } + + int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), remaining_time); + + if (ret > 0) { + if (fds[0].revents & POLLIN) { + const unsigned sleeptime = character_count * 1000000 / (_baudrate / 10); + + int err = 0; + int bytes_available = 0; + err = ::ioctl(_serial_fd, FIONREAD, (unsigned long)&bytes_available); + + if (err != 0 || bytes_available < (int)character_count) { + px4_usleep(sleeptime); + } + + ret = read(&buffer[total_bytes_read], buffer_size - total_bytes_read); + + if (ret > 0) { + total_bytes_read += ret; + } + + } else { + PX4_ERR("Got a poll error"); + return -1; + } + } + } + + return total_bytes_read; +} + +ssize_t SerialImpl::write(const void *buffer, size_t buffer_size) +{ + if (!_open) { + PX4_ERR("Cannot write to serial device until it has been opened"); + return -1; + } + + int written = ::write(_serial_fd, buffer, buffer_size); + ::fsync(_serial_fd); + + if (written < 0) { + PX4_ERR("%s write error %d", _port, written); + } + + return written; +} + +const char *SerialImpl::getPort() const +{ + return _port; +} + +uint32_t SerialImpl::getBaudrate() const +{ + return _baudrate; +} + +bool SerialImpl::setBaudrate(uint32_t baudrate) +{ + if (! validateBaudrate(baudrate)) { + PX4_ERR("ERR: invalid baudrate: %lu", baudrate); + return false; + } + + // check if already configured + if ((baudrate == _baudrate) && _open) { + return true; + } + + _baudrate = baudrate; + + // process baud rate change now if port is already open + if (_open) { + return configure(); + } + + return true; +} + +ByteSize SerialImpl::getBytesize() const +{ + return _bytesize; +} + +bool SerialImpl::setBytesize(ByteSize bytesize) +{ + return bytesize == ByteSize::EightBits; +} + +Parity SerialImpl::getParity() const +{ + return _parity; +} + +bool SerialImpl::setParity(Parity parity) +{ + return parity == Parity::None; +} + +StopBits SerialImpl::getStopbits() const +{ + return _stopbits; +} + +bool SerialImpl::setStopbits(StopBits stopbits) +{ + return stopbits == StopBits::One; +} + +FlowControl SerialImpl::getFlowcontrol() const +{ + return _flowcontrol; +} + +bool SerialImpl::setFlowcontrol(FlowControl flowcontrol) +{ + return flowcontrol == FlowControl::Disabled; +} + +} // namespace device diff --git a/platforms/nuttx/src/px4/common/cdc_acm_check.cpp b/platforms/nuttx/src/px4/common/cdc_acm_check.cpp index d86556894d32..09c77710328a 100644 --- a/platforms/nuttx/src/px4/common/cdc_acm_check.cpp +++ b/platforms/nuttx/src/px4/common/cdc_acm_check.cpp @@ -187,7 +187,7 @@ static void mavlink_usb_check(void *arg) if (param1 == 1) { // 1: Reboot autopilot - px4_reboot_request(false, 0); + px4_reboot_request(REBOOT_REQUEST, 0); } else if (param1 == 2) { // 2: Shutdown autopilot @@ -197,7 +197,7 @@ static void mavlink_usb_check(void *arg) } else if (param1 == 3) { // 3: Reboot autopilot and keep it in the bootloader until upgraded. - px4_reboot_request(true, 0); + px4_reboot_request(REBOOT_TO_BOOTLOADER, 0); } } } diff --git a/platforms/nuttx/src/px4/common/include/SerialImpl.hpp b/platforms/nuttx/src/px4/common/include/SerialImpl.hpp new file mode 100644 index 000000000000..58d41bf759c9 --- /dev/null +++ b/platforms/nuttx/src/px4/common/include/SerialImpl.hpp @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * Copyright (C) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include + +#include + +using device::SerialConfig::ByteSize; +using device::SerialConfig::Parity; +using device::SerialConfig::StopBits; +using device::SerialConfig::FlowControl; + +namespace device +{ + +class SerialImpl +{ +public: + + SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits, + FlowControl flowcontrol); + virtual ~SerialImpl(); + + bool open(); + bool isOpen() const; + + bool close(); + + ssize_t read(uint8_t *buffer, size_t buffer_size); + ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0); + + ssize_t write(const void *buffer, size_t buffer_size); + + const char *getPort() const; + + uint32_t getBaudrate() const; + bool setBaudrate(uint32_t baudrate); + + ByteSize getBytesize() const; + bool setBytesize(ByteSize bytesize); + + Parity getParity() const; + bool setParity(Parity parity); + + StopBits getStopbits() const; + bool setStopbits(StopBits stopbits); + + FlowControl getFlowcontrol() const; + bool setFlowcontrol(FlowControl flowcontrol); + +private: + + int _serial_fd{-1}; + + bool _open{false}; + + char _port[32] {}; + + uint32_t _baudrate{0}; + + ByteSize _bytesize{ByteSize::EightBits}; + Parity _parity{Parity::None}; + StopBits _stopbits{StopBits::One}; + FlowControl _flowcontrol{FlowControl::Disabled}; + + bool validateBaudrate(uint32_t baudrate); + bool configure(); + +}; + +} // namespace device diff --git a/platforms/nuttx/src/px4/common/include/px4_platform/board_determine_hw_info.h b/platforms/nuttx/src/px4/common/include/px4_platform/board_determine_hw_info.h index 95e99ffd46d5..b0864d45c0ec 100644 --- a/platforms/nuttx/src/px4/common/include/px4_platform/board_determine_hw_info.h +++ b/platforms/nuttx/src/px4/common/include/px4_platform/board_determine_hw_info.h @@ -36,6 +36,10 @@ __BEGIN_DECLS #define HW_INFO_SUFFIX "%0" STRINGIFY(HW_INFO_VER_DIGITS) "x%0" STRINGIFY(HW_INFO_REV_DIGITS) "x" +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) +# define HW_INFO_FMUM_SUFFIX "%0" STRINGIFY(HW_INFO_REV_DIGITS) "x" +# define HW_INFO_BASE_SUFFIX "%0" STRINGIFY(HW_INFO_VER_DIGITS) "x" +#endif /************************************************************************************ * Name: board_determine_hw_info * diff --git a/platforms/nuttx/src/px4/common/px4_layer.cmake b/platforms/nuttx/src/px4/common/px4_layer.cmake index 69fd1a007da9..9199a9528997 100644 --- a/platforms/nuttx/src/px4/common/px4_layer.cmake +++ b/platforms/nuttx/src/px4/common/px4_layer.cmake @@ -3,6 +3,8 @@ add_library(px4_layer ${KERNEL_SRCS} cdc_acm_check.cpp + ${PX4_SOURCE_DIR}/platforms/common/Serial.cpp + SerialImpl.cpp ) target_link_libraries(px4_layer diff --git a/platforms/nuttx/src/px4/common/px4_manifest.cpp b/platforms/nuttx/src/px4/common/px4_manifest.cpp index d7402215abdd..f3dc9a5e60bc 100644 --- a/platforms/nuttx/src/px4/common/px4_manifest.cpp +++ b/platforms/nuttx/src/px4/common/px4_manifest.cpp @@ -104,6 +104,13 @@ __EXPORT int px4_mft_query(const px4_mft_s *mft, px4_manifest_types_e type, break; case MFT: + if (mft->mfts[m]->pmft != nullptr) { + system_query_func_t query = (system_query_func_t) mft->mfts[m]->pmft; + return query(sub, val, nullptr); + } + + break; + default: rv = -ENODATA; break; diff --git a/platforms/nuttx/src/px4/common/px4_protected_layers.cmake b/platforms/nuttx/src/px4/common/px4_protected_layers.cmake index 9c57b488b6d3..a1c6ebc39175 100644 --- a/platforms/nuttx/src/px4/common/px4_protected_layers.cmake +++ b/platforms/nuttx/src/px4/common/px4_protected_layers.cmake @@ -15,6 +15,8 @@ add_library(px4_layer usr_board_ctrl.c usr_hrt.cpp usr_mcu_version.cpp + ${PX4_SOURCE_DIR}/platforms/common/Serial.cpp + SerialImpl.cpp ) target_link_libraries(px4_layer diff --git a/platforms/nuttx/src/px4/nxp/imxrt/board_hw_info/board_hw_rev_ver.c b/platforms/nuttx/src/px4/nxp/imxrt/board_hw_info/board_hw_rev_ver.c index 127332feb6f3..e8cfeff32881 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/board_hw_info/board_hw_rev_ver.c +++ b/platforms/nuttx/src/px4/nxp/imxrt/board_hw_info/board_hw_rev_ver.c @@ -50,7 +50,7 @@ #include #include -#if defined(BOARD_HAS_HW_VERSIONING) +#if defined(BOARD_HAS_HW_VERSIONING) || defined(BOARD_HAS_HW_SPLIT_VERSIONING) # if defined(GPIO_HW_VER_REV_DRIVE) # define GPIO_HW_REV_DRIVE GPIO_HW_VER_REV_DRIVE @@ -66,6 +66,9 @@ static int hw_version = 0; static int hw_revision = 0; static char hw_info[HW_INFO_SIZE] = {0}; +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) +static char hw_base_info[HW_INFO_SIZE] = {0}; +#endif /**************************************************************************** * Protected Functions @@ -279,6 +282,27 @@ __EXPORT const char *board_get_hw_type_name() return (const char *) hw_info; } +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) +/************************************************************************************ + * Name: board_get_hw_base_type_name + * + * Description: + * Optional returns a 0 terminated string defining the base 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_base_type_name() +{ + return (const char *) hw_base_info; +} +#endif + /************************************************************************************ * Name: board_get_hw_version * @@ -380,7 +404,12 @@ int board_determine_hw_info() } if (rv == OK) { +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) + snprintf(hw_info, sizeof(hw_info), HW_INFO_INIT_PREFIX HW_INFO_FMUM_SUFFIX, GET_HW_FMUM_ID()); + snprintf(hw_base_info, sizeof(hw_info), HW_INFO_BASE_SUFFIX, GET_HW_BASE_ID()); +#else snprintf(hw_info, sizeof(hw_info), HW_INFO_INIT_PREFIX HW_INFO_SUFFIX, hw_version, hw_revision); +#endif } return rv; diff --git a/platforms/nuttx/src/px4/nxp/imxrt/board_reset/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/imxrt/board_reset/CMakeLists.txt index 06ccd082037c..cba22cb32025 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/board_reset/CMakeLists.txt +++ b/platforms/nuttx/src/px4/nxp/imxrt/board_reset/CMakeLists.txt @@ -35,6 +35,8 @@ px4_add_library(arch_board_reset board_reset.cpp ) +target_link_libraries(arch_board_reset PRIVATE arch_board_romapi) + # up_systemreset if (NOT DEFINED CONFIG_BUILD_FLAT) target_link_libraries(arch_board_reset PRIVATE nuttx_karch) diff --git a/platforms/nuttx/src/px4/nxp/imxrt/board_reset/board_reset.cpp b/platforms/nuttx/src/px4/nxp/imxrt/board_reset/board_reset.cpp index aa54650834f0..6f1e21700d0f 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/board_reset/board_reset.cpp +++ b/platforms/nuttx/src/px4/nxp/imxrt/board_reset/board_reset.cpp @@ -38,11 +38,16 @@ */ #include +#include #include #include #include #include + +#include +#include + #define BOOT_RTC_SIGNATURE 0xb007b007 #define PX4_IMXRT_RTC_REBOOT_REG 3 #define PX4_IMXRT_RTC_REBOOT_REG_ADDRESS IMXRT_SNVS_LPGPR3 @@ -61,8 +66,13 @@ static int board_reset_enter_bootloader() int board_reset(int status) { - if (status == 1) { + if (status == REBOOT_TO_BOOTLOADER) { board_reset_enter_bootloader(); + + } else if (status == REBOOT_TO_ISP) { + uint32_t arg = 0xeb100000; + ROM_API_Init(); + ROM_RunBootloader(&arg); } #if defined(BOARD_HAS_ON_RESET) diff --git a/boards/px4/fmu-v6xrt/src/imxrt_flexspi_nor_flash.h b/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/imxrt_flexspi_nor_flash.h similarity index 98% rename from boards/px4/fmu-v6xrt/src/imxrt_flexspi_nor_flash.h rename to platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/imxrt_flexspi_nor_flash.h index 66a425ee9ecc..727744b974aa 100644 --- a/boards/px4/fmu-v6xrt/src/imxrt_flexspi_nor_flash.h +++ b/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/imxrt_flexspi_nor_flash.h @@ -1,5 +1,5 @@ /**************************************************************************** - * boards/px4/fmu-v6xrt/src/imxrt_flexspi_nor_flash.h + * platforms/nuttx/src/px4/nxp/imrt/include/px4_arch/imxrt_flexspi_nor_flash.h * * Licensed to the Apache Software Foundation (ASF) under one or more * contributor license agreements. See the NOTICE file distributed with @@ -18,8 +18,8 @@ * ****************************************************************************/ -#ifndef __BOARDS_PX4_FMU_V6XRT_SRC_IMXRT_FLEXSPI_NOR_FLASH_H -#define __BOARDS_PX4_FMU_V6XRT_SRC_IMXRT_FLEXSPI_NOR_FLASH_H +#ifndef __PX4_ARCH_IMXRT_FLEXSPI_NOR_FLASH_H +#define __PX4_ARCH_IMXRT_FLEXSPI_NOR_FLASH_H /**************************************************************************** * Included Files @@ -349,4 +349,4 @@ extern const struct flexspi_nor_config_s g_flash_config; extern const struct flexspi_nor_config_s g_flash_fast_config; -#endif /* __BOARDS_PX4_FMU_V6XRT_SRC_IMXRT_FLEXSPI_NOR_FLASH_H */ +#endif /* __PX4_ARCH_IMXRT_FLEXSPI_NOR_FLASH_H */ diff --git a/boards/px4/fmu-v6xrt/src/imxrt_romapi.h b/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/imxrt_romapi.h similarity index 98% rename from boards/px4/fmu-v6xrt/src/imxrt_romapi.h rename to platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/imxrt_romapi.h index de076c68cdae..f51775e2bdca 100644 --- a/boards/px4/fmu-v6xrt/src/imxrt_romapi.h +++ b/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/imxrt_romapi.h @@ -1,14 +1,14 @@ /**************************************************************************** - * boards/px4/fmu-v6xrt/src/imxrt_romapi.c + * platforms/nuttx/src/px4/nxp/imrt/include/px4_arch/imxrt_romapi.h * - * Copyright 2017-2020 NXP + * Copyright 2017-2024 NXP * All rights reserved. * * SPDX-License-Identifier: BSD-3-Clause * ****************************************************************************/ -#ifndef __BOARDS_PX4_FMU_V6XRT_SRC_IMXRT_ROMAPI_H -#define __BOARDS_PX4_FMU_V6XRT_SRC_IMXRT_ROMAPI_H +#ifndef __PX4_ARCH_IMXRT_ROMAPI_H +#define __PX4_ARCH_IMXRT_ROMAPI_H /**************************************************************************** * @@ -370,4 +370,8 @@ void ROM_FLEXSPI_NorFlash_ClearCache(uint32_t instance); /*@}*/ -#endif /* __BOARDS_PX4_FMU_V6XRT_SRC_IMXRT_ROMAPI_H */ +#ifdef __cplusplus +} +#endif + +#endif /* __PX4_ARCH_IMXRT_ROMAPI_H */ diff --git a/platforms/nuttx/src/px4/nxp/imxrt/romapi/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/imxrt/romapi/CMakeLists.txt new file mode 100644 index 000000000000..d6508bdd61d5 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/imxrt/romapi/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_library(arch_board_romapi + imxrt_romapi.c +) diff --git a/boards/px4/fmu-v6xrt/src/imxrt_romapi.c b/platforms/nuttx/src/px4/nxp/imxrt/romapi/imxrt_romapi.c similarity index 98% rename from boards/px4/fmu-v6xrt/src/imxrt_romapi.c rename to platforms/nuttx/src/px4/nxp/imxrt/romapi/imxrt_romapi.c index 0f79d88a0e1e..68443e3353ba 100644 --- a/boards/px4/fmu-v6xrt/src/imxrt_romapi.c +++ b/platforms/nuttx/src/px4/nxp/imxrt/romapi/imxrt_romapi.c @@ -1,7 +1,7 @@ /**************************************************************************** - * boards/px4/fmu-v6xrt/src/imxrt_romapi.c + * platforms/nuttx/src/px4/nxp/imrt/romapi/imxrt_romapi.c * - * Copyright 2017-2020 NXP + * Copyright 2017-2024 NXP * All rights reserved. * * SPDX-License-Identifier: BSD-3-Clause @@ -22,8 +22,8 @@ #include "arm_internal.h" -#include "imxrt_flexspi_nor_flash.h" -#include "imxrt_romapi.h" +#include +#include #include diff --git a/platforms/nuttx/src/px4/nxp/kinetis/board_reset/board_reset.cpp b/platforms/nuttx/src/px4/nxp/kinetis/board_reset/board_reset.cpp index 8a073821ce19..62066a9cc2a6 100644 --- a/platforms/nuttx/src/px4/nxp/kinetis/board_reset/board_reset.cpp +++ b/platforms/nuttx/src/px4/nxp/kinetis/board_reset/board_reset.cpp @@ -38,6 +38,7 @@ */ #include +#include #include #include @@ -72,7 +73,7 @@ static int board_reset_enter_bootloader() int board_reset(int status) { - if (status == 1) { + if (status == REBOOT_TO_BOOTLOADER) { board_reset_enter_bootloader(); } diff --git a/platforms/nuttx/src/px4/nxp/rt117x/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/rt117x/CMakeLists.txt index 8875ab56b776..7a1b5a8bdf9f 100644 --- a/platforms/nuttx/src/px4/nxp/rt117x/CMakeLists.txt +++ b/platforms/nuttx/src/px4/nxp/rt117x/CMakeLists.txt @@ -36,7 +36,7 @@ add_subdirectory(adc) add_subdirectory(../imxrt/board_critmon board_critmon) add_subdirectory(../imxrt/board_hw_info board_hw_info) add_subdirectory(../imxrt/board_reset board_reset) -#add_subdirectory(../imxrt/dshot dshot) +add_subdirectory(../imxrt/romapi romapi) add_subdirectory(../imxrt/hrt hrt) add_subdirectory(../imxrt/led_pwm led_pwm) add_subdirectory(../imxrt/io_pins io_pins) diff --git a/platforms/nuttx/src/px4/nxp/rt117x/include/px4_arch/imxrt_flexspi_nor_flash.h b/platforms/nuttx/src/px4/nxp/rt117x/include/px4_arch/imxrt_flexspi_nor_flash.h new file mode 100644 index 000000000000..3925012e86fd --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/rt117x/include/px4_arch/imxrt_flexspi_nor_flash.h @@ -0,0 +1,36 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +#pragma once + + +#include "../../../imxrt/include/px4_arch/imxrt_flexspi_nor_flash.h" diff --git a/platforms/nuttx/src/px4/nxp/rt117x/include/px4_arch/imxrt_romapi.h b/platforms/nuttx/src/px4/nxp/rt117x/include/px4_arch/imxrt_romapi.h new file mode 100644 index 000000000000..aeab84116295 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/rt117x/include/px4_arch/imxrt_romapi.h @@ -0,0 +1,36 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +#pragma once + + +#include "../../../imxrt/include/px4_arch/imxrt_romapi.h" diff --git a/platforms/nuttx/src/px4/nxp/rt117x/include/px4_arch/spi_hw_description.h b/platforms/nuttx/src/px4/nxp/rt117x/include/px4_arch/spi_hw_description.h index f8573c32f566..64cad11a3b1b 100644 --- a/platforms/nuttx/src/px4/nxp/rt117x/include/px4_arch/spi_hw_description.h +++ b/platforms/nuttx/src/px4/nxp/rt117x/include/px4_arch/spi_hw_description.h @@ -40,6 +40,57 @@ #include +constexpr bool validateSPIConfig(const px4_spi_bus_t spi_busses_conf[SPI_BUS_MAX_BUS_ITEMS]) +{ + const bool nuttx_enabled_spi_buses[] = { +#ifdef CONFIG_IMXRT_LPSPI1 + true, +#else + false, +#endif +#ifdef CONFIG_IMXRT_LPSPI2 + true, +#else + false, +#endif +#ifdef CONFIG_IMXRT_LPSPI3 + true, +#else + false, +#endif +#ifdef CONFIG_IMXRT_LPSPI4 + true, +#else + false, +#endif +#ifdef CONFIG_IMXRT_LPSPI5 + true, +#else + false, +#endif +#ifdef CONFIG_IMXRT_LPSPI6 + true, +#else + false, +#endif + }; + + for (unsigned i = 0; i < sizeof(nuttx_enabled_spi_buses) / sizeof(nuttx_enabled_spi_buses[0]); ++i) { + bool found_bus = false; + + for (int j = 0; j < SPI_BUS_MAX_BUS_ITEMS; ++j) { + if (spi_busses_conf[j].bus == (int)i + 1) { + found_bus = true; + } + } + + // Either the bus is enabled in NuttX and configured in spi_busses_conf, or disabled and not configured + constexpr_assert(found_bus == nuttx_enabled_spi_buses[i], "SPI bus config mismatch (CONFIG_STM32H7_SPIx)"); + } + + return false; +} + static inline constexpr px4_spi_bus_device_t initSPIDevice(uint32_t devid, SPI::CS cs_gpio, SPI::DRDY drdy_gpio = {}) { px4_spi_bus_device_t ret{}; @@ -132,8 +183,56 @@ static inline constexpr SPI::bus_device_external_cfg_t initSPIConfigExternal(SPI return ret; } -constexpr bool validateSPIConfig(const px4_spi_bus_t spi_busses_conf[SPI_BUS_MAX_BUS_ITEMS]) +struct px4_spi_bus_array_t { + px4_spi_bus_t item[SPI_BUS_MAX_BUS_ITEMS]; +}; +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) +static inline constexpr px4_spi_bus_all_hw_t initSPIFmumID(hw_fmun_id_t hw_fmun_id, + const px4_spi_bus_array_t &bus_items) +{ + px4_spi_bus_all_hw_t ret{}; + + for (int i = 0; i < SPI_BUS_MAX_BUS_ITEMS; ++i) { + ret.buses[i] = bus_items.item[i]; + } + + ret.board_hw_fmun_id = hw_fmun_id; + return ret; +} +#else +static inline constexpr px4_spi_bus_all_hw_t initSPIHWVersion(int hw_version_revision, + const px4_spi_bus_array_t &bus_items) { - return true; + px4_spi_bus_all_hw_t ret{}; + + for (int i = 0; i < SPI_BUS_MAX_BUS_ITEMS; ++i) { + ret.buses[i] = bus_items.item[i]; + } + + ret.board_hw_version_revision = hw_version_revision; + return ret; } +#endif +constexpr bool validateSPIConfig(const px4_spi_bus_t spi_buses_conf[SPI_BUS_MAX_BUS_ITEMS]); + +constexpr bool validateSPIConfig(const px4_spi_bus_all_hw_t spi_buses_conf[BOARD_NUM_SPI_CFG_HW_VERSIONS]) +{ + for (int ver = 0; ver < BOARD_NUM_SPI_CFG_HW_VERSIONS; ++ver) { + validateSPIConfig(spi_buses_conf[ver].buses); + } + + for (int ver = 1; ver < BOARD_NUM_SPI_CFG_HW_VERSIONS; ++ver) { + for (int i = 0; i < SPI_BUS_MAX_BUS_ITEMS; ++i) { + const bool equal_power_enable_gpio = spi_buses_conf[ver].buses[i].power_enable_gpio == spi_buses_conf[ver - + 1].buses[i].power_enable_gpio; + // currently board_control_spi_sensors_power_configgpio() depends on that - this restriction can be removed + // by ensuring board_control_spi_sensors_power_configgpio() is called after the hw version is determined + // and SPI config is initialized. + constexpr_assert(equal_power_enable_gpio, "All HW versions must define the same power enable GPIO"); + } + } + + return false; +} + #endif // CONFIG_SPI diff --git a/platforms/nuttx/src/px4/nxp/s32k1xx/board_reset/board_reset.cpp b/platforms/nuttx/src/px4/nxp/s32k1xx/board_reset/board_reset.cpp index 6883d77f555d..98404ffcb8f9 100644 --- a/platforms/nuttx/src/px4/nxp/s32k1xx/board_reset/board_reset.cpp +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/board_reset/board_reset.cpp @@ -39,6 +39,7 @@ */ #include +#include #include #include #include @@ -96,7 +97,7 @@ int board_configure_reset(reset_mode_e mode, uint32_t arg) int board_reset(int status) { - if (status == 1) { + if (status == REBOOT_TO_BOOTLOADER) { board_reset_enter_bootloader(); } diff --git a/platforms/nuttx/src/px4/nxp/s32k3xx/board_reset/board_reset.cpp b/platforms/nuttx/src/px4/nxp/s32k3xx/board_reset/board_reset.cpp index 0607e174c9e7..68fb865e9721 100644 --- a/platforms/nuttx/src/px4/nxp/s32k3xx/board_reset/board_reset.cpp +++ b/platforms/nuttx/src/px4/nxp/s32k3xx/board_reset/board_reset.cpp @@ -39,6 +39,7 @@ */ #include +#include #include #include #include diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/board_reset/board_reset.cpp b/platforms/nuttx/src/px4/rpi/rpi_common/board_reset/board_reset.cpp index 058f07aea118..c5876da85360 100644 --- a/platforms/nuttx/src/px4/rpi/rpi_common/board_reset/board_reset.cpp +++ b/platforms/nuttx/src/px4/rpi/rpi_common/board_reset/board_reset.cpp @@ -38,6 +38,7 @@ */ #include +#include #include #include @@ -119,7 +120,7 @@ int board_reset(int status) { - if (status == 1) { + if (status == REBOOT_TO_BOOTLOADER) { // board_configure_reset(BOARD_RESET_MODE_BOOT_TO_BL, 0); } diff --git a/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/board_hw_rev_ver.c b/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/board_hw_rev_ver.c index 54c8a836c916..6d484c0a826f 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/board_hw_rev_ver.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/board_hw_rev_ver.c @@ -51,7 +51,7 @@ #include #include -#if defined(BOARD_HAS_HW_VERSIONING) +#if defined(BOARD_HAS_HW_VERSIONING) || defined(BOARD_HAS_HW_SPLIT_VERSIONING) # if defined(GPIO_HW_VER_REV_DRIVE) # define GPIO_HW_REV_DRIVE GPIO_HW_VER_REV_DRIVE @@ -67,7 +67,9 @@ static int hw_version = 0; static int hw_revision = 0; static char hw_info[HW_INFO_SIZE] = {0}; - +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) +static char hw_base_info[HW_INFO_SIZE] = {0}; +#endif /**************************************************************************** * Protected Functions ****************************************************************************/ @@ -366,6 +368,27 @@ __EXPORT const char *board_get_hw_type_name() return (const char *) hw_info; } +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) +/************************************************************************************ + * Name: board_get_hw_base_type_name + * + * Description: + * Optional returns a 0 terminated string defining the base 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_base_type_name() +{ + return (const char *) hw_base_info; +} +#endif + /************************************************************************************ * Name: board_get_hw_version * @@ -467,7 +490,12 @@ int board_determine_hw_info() } if (rv == OK) { +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) + snprintf(hw_info, sizeof(hw_info), HW_INFO_INIT_PREFIX HW_INFO_FMUM_SUFFIX, GET_HW_FMUM_ID()); + snprintf(hw_base_info, sizeof(hw_info), HW_INFO_BASE_SUFFIX, GET_HW_BASE_ID()); +#else snprintf(hw_info, sizeof(hw_info), HW_INFO_INIT_PREFIX HW_INFO_SUFFIX, hw_version, hw_revision); +#endif } return rv; diff --git a/platforms/nuttx/src/px4/stm/stm32_common/board_reset/board_reset.cpp b/platforms/nuttx/src/px4/stm/stm32_common/board_reset/board_reset.cpp index 15dfbd87616a..28b5ba5f848f 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/board_reset/board_reset.cpp +++ b/platforms/nuttx/src/px4/stm/stm32_common/board_reset/board_reset.cpp @@ -38,6 +38,7 @@ */ #include +#include #include #include #include @@ -117,7 +118,7 @@ int board_configure_reset(reset_mode_e mode, uint32_t arg) int board_reset(int status) { - if (status == 1) { + if (status == REBOOT_TO_BOOTLOADER) { board_configure_reset(BOARD_RESET_MODE_BOOT_TO_BL, 0); } diff --git a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/spi_hw_description.h b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/spi_hw_description.h index 40cc40854b7c..3594d1d82011 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/spi_hw_description.h +++ b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/spi_hw_description.h @@ -136,6 +136,21 @@ static inline constexpr SPI::bus_device_external_cfg_t initSPIConfigExternal(SPI struct px4_spi_bus_array_t { px4_spi_bus_t item[SPI_BUS_MAX_BUS_ITEMS]; }; + +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) +static inline constexpr px4_spi_bus_all_hw_t initSPIFmumID(hw_fmun_id_t hw_fmun_id, + const px4_spi_bus_array_t &bus_items) +{ + px4_spi_bus_all_hw_t ret{}; + + for (int i = 0; i < SPI_BUS_MAX_BUS_ITEMS; ++i) { + ret.buses[i] = bus_items.item[i]; + } + + ret.board_hw_fmun_id = hw_fmun_id; + return ret; +} +#else static inline constexpr px4_spi_bus_all_hw_t initSPIHWVersion(int hw_version_revision, const px4_spi_bus_array_t &bus_items) { @@ -148,6 +163,7 @@ static inline constexpr px4_spi_bus_all_hw_t initSPIHWVersion(int hw_version_rev ret.board_hw_version_revision = hw_version_revision; return ret; } +#endif constexpr bool validateSPIConfig(const px4_spi_bus_t spi_buses_conf[SPI_BUS_MAX_BUS_ITEMS]); constexpr bool validateSPIConfig(const px4_spi_bus_all_hw_t spi_buses_conf[BOARD_NUM_SPI_CFG_HW_VERSIONS]) diff --git a/platforms/nuttx/src/px4/stm/stm32h7/adc/adc.cpp b/platforms/nuttx/src/px4/stm/stm32h7/adc/adc.cpp index 80f98c564fd1..2a25cf6eeed5 100644 --- a/platforms/nuttx/src/px4/stm/stm32h7/adc/adc.cpp +++ b/platforms/nuttx/src/px4/stm/stm32h7/adc/adc.cpp @@ -37,6 +37,7 @@ #include #include +#include "stm32_dbgmcu.h" #include #include @@ -96,35 +97,123 @@ #define ADC_MAX_FADC 36000000 -#if STM32_PLL2P_FREQUENCY <= ADC_MAX_FADC -# define ADC_CCR_PRESC_DIV ADC_CCR_PRESC_NOT_DIV -#elif STM32_PLL2P_FREQUENCY/2 <= ADC_MAX_FADC -# define ADC_CCR_PRESC_DIV ADC_CCR_PRESC_DIV2 -#elif STM32_PLL2P_FREQUENCY/4 <= ADC_MAX_FADC -# define ADC_CCR_PRESC_DIV ADC_CCR_PRESC_DIV4 -#elif STM32_PLL2P_FREQUENCY/6 <= ADC_MAX_FADC -# define ADC_CCR_PRESC_DIV ADC_CCR_PRESC_DIV6 -#elif STM32_PLL2P_FREQUENCY/8 <= ADC_MAX_FADC -# define ADC_CCR_PRESC_DIV ADC_CCR_PRESC_DIV8 -#elif STM32_PLL2P_FREQUENCY/10 <= ADC_MAX_FADC -# define ADC_CCR_PRESC_DIV ADC_CCR_PRESC_DIV10 -#elif STM32_PLL2P_FREQUENCY/12 <= ADC_MAX_FADC -# define ADC_CCR_PRESC_DIV ADC_CCR_PRESC_DIV12 -#elif STM32_PLL2P_FREQUENCY/16 <= ADC_MAX_FADC -# define ADC_CCR_PRESC_DIV ADC_CCR_PRESC_DIV16 -#elif STM32_PLL2P_FREQUENCY/32 <= ADC_MAX_FADC -# define ADC_CCR_PRESC_DIV ADC_CCR_PRESC_DIV32 -#elif STM32_PLL2P_FREQUENCY/64 <= ADC_MAX_FADC -# define ADC_CCR_PRESC_DIV ADC_CCR_PRESC_DIV64 -#elif STM32_PLL2P_FREQUENCY/128 <= ADC_MAX_FADC -# define ADC_CCR_PRESC_DIV ADC_CCR_PRESC_DIV128 -#elif STM32_PLL2P_FREQUENCY/256 <= ADC_MAX_FADC -# define ADC_CCR_PRESC_DIV ADC_CCR_PRESC_DIV256 -#else -# error "ADC STM32_PLL2P_FREQUENCY too high - no divisor found " +#define ADC3_INTERNAL_TEMP_SENSOR_CHANNEL 18 //define to map the internal temperature channel. + + +/**************************************************************************** + * Name: adc_getclocks + ****************************************************************************/ + +static int adc_getclocks(uint32_t *prescaler, uint32_t *boost) +{ + uint32_t max_clock = ADC_MAX_FADC; + uint32_t src_clock = STM32_PLL2P_FREQUENCY; + uint32_t adc_clock; + int rv = OK; + +#if STM32_RCC_D3CCIPR_ADCSRC == RCC_D3CCIPR_ADCSEL_PLL3 + src_clock = STM32_PLL3R_FREQUENCY; +#elif STM32_RCC_D3CCIPR_ADCSRC == RCC_D3CCIPR_ADCSEL_PER +# error ADCSEL_PER not supported #endif -#define ADC3_INTERNAL_TEMP_SENSOR_CHANNEL 18 //define to map the internal temperature channel. + /* The maximum clock is different for rev Y devices and rev V devices. + * rev V can support an ADC clock of up to 50MHz. rev Y only supports + * up to 36MHz. + */ + + if ((getreg32(STM32_DEBUGMCU_BASE) & DBGMCU_IDCODE_REVID_MASK) == + STM32_IDCODE_REVID_V) { + /* The max fadc is 50MHz, but there is an always-present /2 divider + * after the configurable prescaler. Therefore, the max clock out of + * the prescaler block is 2*50=100MHz + */ + + max_clock = 100000000; + } + + if (src_clock <= max_clock) { + *prescaler = ADC_CCR_PRESC_NOT_DIV; + adc_clock = src_clock; + + } else if (src_clock / 2 <= max_clock) { + *prescaler = ADC_CCR_PRESC_DIV2; + adc_clock = src_clock / 2; + + } else if (src_clock / 4 <= max_clock) { + *prescaler = ADC_CCR_PRESC_DIV4; + adc_clock = src_clock / 4; + + } else if (src_clock / 6 <= max_clock) { + *prescaler = ADC_CCR_PRESC_DIV6; + adc_clock = src_clock / 6; + + } else if (src_clock / 8 <= max_clock) { + *prescaler = ADC_CCR_PRESC_DIV8; + adc_clock = src_clock / 8; + + } else if (src_clock / 10 <= max_clock) { + *prescaler = ADC_CCR_PRESC_DIV10; + adc_clock = src_clock / 10; + + } else if (src_clock / 12 <= max_clock) { + *prescaler = ADC_CCR_PRESC_DIV12; + adc_clock = src_clock / 12; + + } else if (src_clock / 16 <= max_clock) { + *prescaler = ADC_CCR_PRESC_DIV16; + adc_clock = src_clock / 16; + + } else if (src_clock / 32 <= max_clock) { + *prescaler = ADC_CCR_PRESC_DIV32; + adc_clock = src_clock / 32; + + } else if (src_clock / 64 <= max_clock) { + *prescaler = ADC_CCR_PRESC_DIV64; + adc_clock = src_clock / 64; + + } else if (src_clock / 128 <= max_clock) { + *prescaler = ADC_CCR_PRESC_DIV128; + adc_clock = src_clock / 128; + + } else if (src_clock / 256 <= max_clock) { + *prescaler = ADC_CCR_PRESC_DIV256; + adc_clock = src_clock / 256; + + } else { + rv = -1; + } + + if ((getreg32(STM32_DEBUGMCU_BASE) & DBGMCU_IDCODE_REVID_MASK) == + STM32_IDCODE_REVID_V) { + if (adc_clock >= 25000000) { + *boost = ADC_CR_BOOST_50_MHZ; + + } else if (adc_clock >= 12500000) { + *boost = ADC_CR_BOOST_25_MHZ; + + } else if (adc_clock >= 6250000) { + *boost = ADC_CR_BOOST_12p5_MHZ; + + } else { + *boost = ADC_CR_BOOST_6p25_MHZ; + } + + } else { + if (adc_clock >= 20000000) { + *boost = ADC_CR_BOOST; + + } else { + *boost = 0; + } + } + + return rv; +} + +/**************************************************************************** + * Name: px4_arch_adc_init + ****************************************************************************/ int px4_arch_adc_init(uint32_t base_address) { @@ -158,9 +247,21 @@ int px4_arch_adc_init(uint32_t base_address) *free = base_address; + /* Get cloking for this version of Siicon */ + + uint32_t boost = ADC_CR_BOOST; + uint32_t prescaler = ADC_CCR_PRESC_DIV256; + + if (adc_getclocks(&prescaler, &boost) != OK) { + + /* ERROR: source clock too high */ + + PANIC(); + } + /* do calibration if supported */ - rCR(base_address) = ADC_CR_ADVREGEN | ADC_CR_BOOST; + rCR(base_address) = ADC_CR_ADVREGEN | boost; /* Wait for voltage regulator to power up */ @@ -169,7 +270,7 @@ int px4_arch_adc_init(uint32_t base_address) /* enable the temperature sensor, VREFINT channel and VBAT */ rCCR(base_address) = (ADC_CCR_VREFEN | ADC_CCR_VSENSEEN | ADC_CCR_VBATEN | - ADC_CCR_CKMODE_ASYCH | ADC_CCR_PRESC_DIV); + ADC_CCR_CKMODE_ASYCH | prescaler); /* Enable ADC calibration. ADCALDIF == 0 so this is only for * single-ended conversions, not for differential ones. diff --git a/platforms/posix/cmake/Toolchain-aarch64-linux-gnu.cmake b/platforms/posix/cmake/Toolchain-aarch64-linux-gnu.cmake index 2a7cb6c8a372..d48cfc95f199 100644 --- a/platforms/posix/cmake/Toolchain-aarch64-linux-gnu.cmake +++ b/platforms/posix/cmake/Toolchain-aarch64-linux-gnu.cmake @@ -2,16 +2,8 @@ set(triple aarch64-linux-gnu) -if("${PX4_BOARD}" MATCHES "modalai_voxl2") - set(CMAKE_LIBRARY_ARCHITECTURE ${ARM_CROSS_GCC_ROOT}/bin/${triple}) - set(TOOLCHAIN_PREFIX ${ARM_CROSS_GCC_ROOT}/bin/${triple}) - set(ARM_CROSS_GCC_ROOT $ENV{ARM_CROSS_GCC_ROOT}) - set(HEXAGON_ARM_SYSROOT $ENV{HEXAGON_ARM_SYSROOT}) - set(CMAKE_EXE_LINKER_FLAGS "-Wl,-gc-sections -Wl,-rpath-link,${HEXAGON_ARM_SYSROOT}/usr/lib/aarch64-linux-gnu -Wl,-rpath-link,${HEXAGON_ARM_SYSROOT}/lib/aarch64-linux-gnu") -else() - set(CMAKE_LIBRARY_ARCHITECTURE ${triple}) - set(TOOLCHAIN_PREFIX ${triple}) -endif() +set(CMAKE_LIBRARY_ARCHITECTURE ${triple}) +set(TOOLCHAIN_PREFIX ${triple}) set(CMAKE_SYSTEM_NAME Linux) set(CMAKE_SYSTEM_PROCESSOR arm) diff --git a/platforms/posix/include/SerialImpl.hpp b/platforms/posix/include/SerialImpl.hpp new file mode 100644 index 000000000000..efc95d7d517a --- /dev/null +++ b/platforms/posix/include/SerialImpl.hpp @@ -0,0 +1,103 @@ +/**************************************************************************** + * + * Copyright (C) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include + +#include + +using device::SerialConfig::ByteSize; +using device::SerialConfig::Parity; +using device::SerialConfig::StopBits; +using device::SerialConfig::FlowControl; + +namespace device +{ + +class SerialImpl +{ +public: + + SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits, + FlowControl flowcontrol); + virtual ~SerialImpl(); + + bool open(); + bool isOpen() const; + + bool close(); + + ssize_t read(uint8_t *buffer, size_t buffer_size); + ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0); + + ssize_t write(const void *buffer, size_t buffer_size); + + const char *getPort() const; + + uint32_t getBaudrate() const; + bool setBaudrate(uint32_t baudrate); + + ByteSize getBytesize() const; + bool setBytesize(ByteSize bytesize); + + Parity getParity() const; + bool setParity(Parity parity); + + StopBits getStopbits() const; + bool setStopbits(StopBits stopbits); + + FlowControl getFlowcontrol() const; + bool setFlowcontrol(FlowControl flowcontrol); + +private: + + int _serial_fd{-1}; + + bool _open{false}; + + char _port[32] {}; + + uint32_t _baudrate{0}; + + ByteSize _bytesize{ByteSize::EightBits}; + Parity _parity{Parity::None}; + StopBits _stopbits{StopBits::One}; + FlowControl _flowcontrol{FlowControl::Disabled}; + + bool validateBaudrate(uint32_t baudrate); + bool configure(); +}; + +} // namespace device diff --git a/platforms/posix/src/px4/common/CMakeLists.txt b/platforms/posix/src/px4/common/CMakeLists.txt index 34b65cdf44aa..90d4a77992bb 100644 --- a/platforms/posix/src/px4/common/CMakeLists.txt +++ b/platforms/posix/src/px4/common/CMakeLists.txt @@ -46,6 +46,8 @@ add_library(px4_layer drv_hrt.cpp cpuload.cpp print_load.cpp + ${PX4_SOURCE_DIR}/platforms/common/Serial.cpp + SerialImpl.cpp ) target_compile_definitions(px4_layer PRIVATE MODULE_NAME="px4") target_compile_options(px4_layer PRIVATE -Wno-cast-align) # TODO: fix and enable diff --git a/platforms/posix/src/px4/common/SerialImpl.cpp b/platforms/posix/src/px4/common/SerialImpl.cpp new file mode 100644 index 000000000000..79e3422aedf2 --- /dev/null +++ b/platforms/posix/src/px4/common/SerialImpl.cpp @@ -0,0 +1,387 @@ +/**************************************************************************** + * + * Copyright (C) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include // strncpy +#include +#include +#include +#include +#include +#include + +namespace device +{ + +SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits, + FlowControl flowcontrol) : + _baudrate(baudrate), + _bytesize(bytesize), + _parity(parity), + _stopbits(stopbits), + _flowcontrol(flowcontrol) +{ + if (port) { + strncpy(_port, port, sizeof(_port) - 1); + _port[sizeof(_port) - 1] = '\0'; + + } else { + _port[0] = 0; + } +} + +SerialImpl::~SerialImpl() +{ + if (isOpen()) { + close(); + } +} + +bool SerialImpl::validateBaudrate(uint32_t baudrate) +{ + return ((baudrate == 9600) || + (baudrate == 19200) || + (baudrate == 38400) || + (baudrate == 57600) || + (baudrate == 115200) || + (baudrate == 230400) || + (baudrate == 460800) || + (baudrate == 921600)); +} + +bool SerialImpl::configure() +{ + /* process baud rate */ + int speed; + + if (! validateBaudrate(_baudrate)) { + PX4_ERR("ERR: unknown baudrate: %u", _baudrate); + return false; + } + + switch (_baudrate) { + case 9600: speed = B9600; break; + + case 19200: speed = B19200; break; + + case 38400: speed = B38400; break; + + case 57600: speed = B57600; break; + + case 115200: speed = B115200; break; + + case 230400: speed = B230400; break; + +#ifndef B460800 +#define B460800 460800 +#endif + + case 460800: speed = B460800; break; + +#ifndef B921600 +#define B921600 921600 +#endif + + case 921600: speed = B921600; break; + + default: + PX4_ERR("ERR: unknown baudrate: %d", _baudrate); + return false; + } + + struct termios uart_config; + + int termios_state; + + /* fill the struct for the new configuration */ + if ((termios_state = tcgetattr(_serial_fd, &uart_config)) < 0) { + PX4_ERR("ERR: %d (tcgetattr)", termios_state); + return false; + } + + /* properly configure the terminal (see also https://en.wikibooks.org/wiki/Serial_Programming/termios ) */ + + // + // Input flags - Turn off input processing + // + // convert break to null byte, no CR to NL translation, + // no NL to CR translation, don't mark parity errors or breaks + // no input parity check, don't strip high bit off, + // no XON/XOFF software flow control + // + uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | + INLCR | PARMRK | INPCK | ISTRIP | IXON); + + // + // Output flags - Turn off output processing + // + // no CR to NL translation, no NL to CR-NL translation, + // no NL to CR translation, no column 0 CR suppression, + // no Ctrl-D suppression, no fill characters, no case mapping, + // no local output processing + // + // config.c_oflag &= ~(OCRNL | ONLCR | ONLRET | + // ONOCR | ONOEOT| OFILL | OLCUC | OPOST); + uart_config.c_oflag = 0; + + // + // No line processing + // + // echo off, echo newline off, canonical mode off, + // extended input processing off, signal chars off + // + uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG); + + /* no parity, one stop bit, disable flow control */ + uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS); + + /* set baud rate */ + if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { + PX4_ERR("ERR: %d (cfsetispeed)", termios_state); + return false; + } + + if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { + PX4_ERR("ERR: %d (cfsetospeed)", termios_state); + return false; + } + + if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) { + PX4_ERR("ERR: %d (tcsetattr)", termios_state); + return false; + } + + return true; +} + +bool SerialImpl::open() +{ + if (isOpen()) { + return true; + } + + // Open the serial port + int serial_fd = ::open(_port, O_RDWR | O_NOCTTY); + + if (serial_fd < 0) { + PX4_ERR("failed to open %s err: %d", _port, errno); + return false; + } + + _serial_fd = serial_fd; + + // Configure the serial port + if (! configure()) { + PX4_ERR("failed to configure %s err: %d", _port, errno); + return false; + } + + _open = true; + + return _open; +} + +bool SerialImpl::isOpen() const +{ + return _open; +} + +bool SerialImpl::close() +{ + + if (_serial_fd >= 0) { + ::close(_serial_fd); + } + + _serial_fd = -1; + _open = false; + + return true; +} + +ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size) +{ + if (!_open) { + PX4_ERR("Cannot read from serial device until it has been opened"); + return -1; + } + + int ret = ::read(_serial_fd, buffer, buffer_size); + + if (ret < 0) { + PX4_DEBUG("%s read error %d", _port, ret); + + } + + return ret; +} + +ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us) +{ + if (!_open) { + PX4_ERR("Cannot readAtLeast from serial device until it has been opened"); + return -1; + } + + if (buffer_size < character_count) { + PX4_ERR("%s: Buffer not big enough to hold desired amount of read data", __FUNCTION__); + return -1; + } + + const hrt_abstime start_time_us = hrt_absolute_time(); + int total_bytes_read = 0; + + while ((total_bytes_read < (int) character_count) && (hrt_elapsed_time(&start_time_us) < timeout_us)) { + // Poll for incoming UART data. + pollfd fds[1]; + fds[0].fd = _serial_fd; + fds[0].events = POLLIN; + + hrt_abstime remaining_time = timeout_us - hrt_elapsed_time(&start_time_us); + + if (remaining_time <= 0) { break; } + + int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), remaining_time); + + if (ret > 0) { + if (fds[0].revents & POLLIN) { + const unsigned sleeptime = character_count * 1000000 / (_baudrate / 10); + px4_usleep(sleeptime); + + ret = read(&buffer[total_bytes_read], buffer_size - total_bytes_read); + + if (ret > 0) { + total_bytes_read += ret; + } + + } else { + PX4_ERR("Got a poll error"); + return -1; + } + } + } + + return total_bytes_read; +} + +ssize_t SerialImpl::write(const void *buffer, size_t buffer_size) +{ + if (!_open) { + PX4_ERR("Cannot write to serial device until it has been opened"); + return -1; + } + + int written = ::write(_serial_fd, buffer, buffer_size); + ::fsync(_serial_fd); + + if (written < 0) { + PX4_ERR("%s write error %d", _port, written); + + } + + return written; +} + +const char *SerialImpl::getPort() const +{ + return _port; +} + +uint32_t SerialImpl::getBaudrate() const +{ + return _baudrate; +} + +bool SerialImpl::setBaudrate(uint32_t baudrate) +{ + if (! validateBaudrate(baudrate)) { + PX4_ERR("ERR: invalid baudrate: %u", baudrate); + return false; + } + + // check if already configured + if ((baudrate == _baudrate) && _open) { + return true; + } + + _baudrate = baudrate; + + // process baud rate change now if port is already open + if (_open) { + return configure(); + } + + return true; +} + +ByteSize SerialImpl::getBytesize() const +{ + return _bytesize; +} + +bool SerialImpl::setBytesize(ByteSize bytesize) +{ + return bytesize == ByteSize::EightBits; +} + +Parity SerialImpl::getParity() const +{ + return _parity; +} + +bool SerialImpl::setParity(Parity parity) +{ + return parity == Parity::None; +} + +StopBits SerialImpl::getStopbits() const +{ + return _stopbits; +} + +bool SerialImpl::setStopbits(StopBits stopbits) +{ + return stopbits == StopBits::One; +} + +FlowControl SerialImpl::getFlowcontrol() const +{ + return _flowcontrol; +} + +bool SerialImpl::setFlowcontrol(FlowControl flowcontrol) +{ + return flowcontrol == FlowControl::Disabled; +} + +} // namespace device diff --git a/platforms/posix/src/px4/common/drv_hrt.cpp b/platforms/posix/src/px4/common/drv_hrt.cpp index 9634511bf3fc..b510229bc0cf 100644 --- a/platforms/posix/src/px4/common/drv_hrt.cpp +++ b/platforms/posix/src/px4/common/drv_hrt.cpp @@ -51,6 +51,11 @@ #include #include "hrt_work.h" +// Voxl2 board specific API definitions to get time offset +#if defined(CONFIG_MUORB_APPS_SYNC_TIMESTAMP) +#include "fc_sensor.h" +#endif + #if defined(ENABLE_LOCKSTEP_SCHEDULER) #include static LockstepScheduler lockstep_scheduler {true}; @@ -107,6 +112,29 @@ hrt_abstime hrt_absolute_time() #else // defined(ENABLE_LOCKSTEP_SCHEDULER) struct timespec ts; px4_clock_gettime(CLOCK_MONOTONIC, &ts); + +# if defined(CONFIG_MUORB_APPS_SYNC_TIMESTAMP) + hrt_abstime temp_abstime = ts_to_abstime(&ts); + int apps_time_offset = fc_sensor_get_time_offset(); + + if (apps_time_offset < 0) { + hrt_abstime temp_offset = -apps_time_offset; + + if (temp_offset >= temp_abstime) { + temp_abstime = 0; + + } else { + temp_abstime -= temp_offset; + } + + } else { + temp_abstime += (hrt_abstime) apps_time_offset; + } + + ts.tv_sec = temp_abstime / 1000000; + ts.tv_nsec = (temp_abstime % 1000000) * 1000; +# endif // defined(CONFIG_MUORB_APPS_SYNC_TIMESTAMP) + return ts_to_abstime(&ts); #endif // defined(ENABLE_LOCKSTEP_SCHEDULER) } @@ -449,6 +477,7 @@ int px4_clock_gettime(clockid_t clk_id, struct timespec *tp) #endif // defined(ENABLE_LOCKSTEP_SCHEDULER) return system_clock_gettime(clk_id, tp); + } #if defined(ENABLE_LOCKSTEP_SCHEDULER) diff --git a/platforms/posix/src/px4/common/px4_init.cpp b/platforms/posix/src/px4/common/px4_init.cpp index b9989a54e07a..239841019263 100644 --- a/platforms/posix/src/px4/common/px4_init.cpp +++ b/platforms/posix/src/px4/common/px4_init.cpp @@ -40,15 +40,34 @@ #include #include +#if defined(CONFIG_MODULES_MUORB_APPS) +extern "C" { int muorb_init(); } +#endif + int px4_platform_init(void) { hrt_init(); px4::WorkQueueManagerStart(); +// MUORB has slightly different startup requirements +#if defined(CONFIG_MODULES_MUORB_APPS) + //Put sleeper in here to allow wq to finish initializing before param_init is called + usleep(10000); + + uorb_start(); + + muorb_init(); + + // Give muorb some time to setup the DSP + usleep(100000); + + param_init(); +#else param_init(); uorb_start(); +#endif px4_log_initialize(); diff --git a/platforms/qurt/CMakeLists.txt b/platforms/qurt/CMakeLists.txt index d85f3fcb7bff..0e9757a5d1a9 100644 --- a/platforms/qurt/CMakeLists.txt +++ b/platforms/qurt/CMakeLists.txt @@ -50,5 +50,4 @@ add_library(px4 SHARED target_link_libraries(px4 modules__muorb__slpi ${module_libraries} - px4_layer ) diff --git a/platforms/qurt/cmake/qurt_reqs.cmake b/platforms/qurt/cmake/qurt_reqs.cmake index c8920b717a7e..37be2f72d434 100644 --- a/platforms/qurt/cmake/qurt_reqs.cmake +++ b/platforms/qurt/cmake/qurt_reqs.cmake @@ -121,7 +121,8 @@ exec_program(${CMAKE_CXX_COMPILER} ${CMAKE_CURRENT_SOURCE_DIR} ARGS -print-file- set(EXTRA_LIBS ${EXTRA_LIBS} ${LIBM}) # Full optimization and Link Time Optimization (LTO) -set(QURT_OPTIMIZATION_FLAGS -O3 -flto) +# set(QURT_OPTIMIZATION_FLAGS -O3 -flto) +set(QURT_OPTIMIZATION_FLAGS -O3) # LTO takes a lot of extra time and doesn't really help much... # Flags we pass to the C compiler list2string(CFLAGS diff --git a/platforms/qurt/include/SerialImpl.hpp b/platforms/qurt/include/SerialImpl.hpp new file mode 100644 index 000000000000..1b98d3bb401b --- /dev/null +++ b/platforms/qurt/include/SerialImpl.hpp @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * Copyright (C) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +#include + +using device::SerialConfig::ByteSize; +using device::SerialConfig::Parity; +using device::SerialConfig::StopBits; +using device::SerialConfig::FlowControl; + +namespace device +{ + +class SerialImpl +{ +public: + + SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits, + FlowControl flowcontrol); + virtual ~SerialImpl(); + + bool open(); + bool isOpen() const; + + bool close(); + + ssize_t read(uint8_t *buffer, size_t buffer_size); + ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0); + + ssize_t write(const void *buffer, size_t buffer_size); + + const char *getPort() const; + bool setPort(const char *port); + + uint32_t getBaudrate() const; + bool setBaudrate(uint32_t baudrate); + + ByteSize getBytesize() const; + bool setBytesize(ByteSize bytesize); + + Parity getParity() const; + bool setParity(Parity parity); + + StopBits getStopbits() const; + bool setStopbits(StopBits stopbits); + + FlowControl getFlowcontrol() const; + bool setFlowcontrol(FlowControl flowcontrol); + +private: + + int _serial_fd{-1}; + + bool _open{false}; + + char _port[32] {}; + + uint32_t _baudrate{0}; + + ByteSize _bytesize{ByteSize::EightBits}; + Parity _parity{Parity::None}; + StopBits _stopbits{StopBits::One}; + FlowControl _flowcontrol{FlowControl::Disabled}; + + bool validateBaudrate(uint32_t baudrate); + + // Mutex used to lock the read functions + //pthread_mutex_t read_mutex; + + // Mutex used to lock the write functions + //pthread_mutex_t write_mutex; +}; + +} // namespace device diff --git a/platforms/qurt/src/px4/CMakeLists.txt b/platforms/qurt/src/px4/CMakeLists.txt index e685b8d42acf..8a25322755a9 100644 --- a/platforms/qurt/src/px4/CMakeLists.txt +++ b/platforms/qurt/src/px4/CMakeLists.txt @@ -38,6 +38,7 @@ set(QURT_LAYER_SRCS px4_qurt_impl.cpp main.cpp qurt_log.cpp + SerialImpl.cpp ) add_library(px4_layer diff --git a/platforms/qurt/src/px4/SerialImpl.cpp b/platforms/qurt/src/px4/SerialImpl.cpp new file mode 100644 index 000000000000..ec0fb73fce3a --- /dev/null +++ b/platforms/qurt/src/px4/SerialImpl.cpp @@ -0,0 +1,326 @@ +/**************************************************************************** + * + * Copyright (C) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include // strncpy +#include +#include +#include + +namespace device +{ + +SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits, + FlowControl flowcontrol) : + _baudrate(baudrate), + _bytesize(bytesize), + _parity(parity), + _stopbits(stopbits), + _flowcontrol(flowcontrol) +{ + if (port) { + strncpy(_port, port, sizeof(_port) - 1); + _port[sizeof(_port) - 1] = '\0'; + + } else { + _port[0] = 0; + } +} + +SerialImpl::~SerialImpl() +{ + if (isOpen()) { + close(); + } +} + +bool SerialImpl::validateBaudrate(uint32_t baudrate) +{ + if ((baudrate != 9600) && + (baudrate != 38400) && + (baudrate != 57600) && + (baudrate != 115200) && + (baudrate != 230400) && + (baudrate != 250000) && + (baudrate != 420000) && + (baudrate != 460800) && + (baudrate != 921600) && + (baudrate != 1000000) && + (baudrate != 1843200) && + (baudrate != 2000000)) { + return false; + } + + return true; +} + +bool SerialImpl::open() +{ + // There's no harm in calling open multiple times on the same port. + // In fact, that's the only way to change the baudrate + + _open = false; + _serial_fd = -1; + + if (! validateBaudrate(_baudrate)) { + PX4_ERR("Invalid baudrate: %u", _baudrate); + return false; + } + + if (_bytesize != ByteSize::EightBits) { + PX4_ERR("Qurt platform only supports ByteSize::EightBits"); + return false; + } + + if (_parity != Parity::None) { + PX4_ERR("Qurt platform only supports Parity::None"); + return false; + } + + if (_stopbits != StopBits::One) { + PX4_ERR("Qurt platform only supports StopBits::One"); + return false; + } + + if (_flowcontrol != FlowControl::Disabled) { + PX4_ERR("Qurt platform only supports FlowControl::Disabled"); + return false; + } + + // qurt_uart_open will check validity of port and baudrate + int serial_fd = qurt_uart_open(_port, _baudrate); + + if (serial_fd < 0) { + PX4_ERR("failed to open %s, fd returned: %d", _port, serial_fd); + return false; + + } else { + PX4_INFO("Successfully opened UART %s with baudrate %u", _port, _baudrate); + } + + _serial_fd = serial_fd; + _open = true; + + return _open; +} + +bool SerialImpl::isOpen() const +{ + return _open; +} + +bool SerialImpl::close() +{ + // No close defined for qurt uart yet + // if (_serial_fd >= 0) { + // qurt_uart_close(_serial_fd); + // } + + _serial_fd = -1; + _open = false; + + return true; +} + +ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size) +{ + if (!_open) { + PX4_ERR("Cannot read from serial device until it has been opened"); + return -1; + } + + int ret_read = qurt_uart_read(_serial_fd, (char *) buffer, buffer_size, 500); + + if (ret_read < 0) { + PX4_DEBUG("%s read error %d", _port, ret_read); + + } + + return ret_read; +} + +ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us) +{ + if (!_open) { + PX4_ERR("Cannot readAtLeast from serial device until it has been opened"); + return -1; + } + + if (buffer_size < character_count) { + PX4_ERR("%s: Buffer not big enough to hold desired amount of read data", __FUNCTION__); + return -1; + } + + const hrt_abstime start_time_us = hrt_absolute_time(); + int total_bytes_read = 0; + + while (total_bytes_read < (int) character_count) { + + if (timeout_us > 0) { + const uint64_t elapsed_us = hrt_elapsed_time(&start_time_us); + + if (elapsed_us >= timeout_us) { + // If there was a partial read but not enough to satisfy the minimum then they will be lost + // but this really should never happen when everything is working normally. + // PX4_WARN("%s timeout %d bytes read (%llu us elapsed)", __FUNCTION__, total_bytes_read, elapsed_us); + // Or, instead of returning an error, should we return the number of bytes read (assuming it is greater than zero)? + return total_bytes_read; + } + } + + int current_bytes_read = read(&buffer[total_bytes_read], buffer_size - total_bytes_read); + + if (current_bytes_read < 0) { + // Again, if there was a partial read but not enough to satisfy the minimum then they will be lost + // but this really should never happen when everything is working normally. + PX4_ERR("%s failed to read uart", __FUNCTION__); + // Or, instead of returning an error, should we return the number of bytes read (assuming it is greater than zero)? + return -1; + } + + // Current bytes read could be zero + total_bytes_read += current_bytes_read; + + // If we have at least reached our desired minimum number of characters + // then we can return now + if (total_bytes_read >= (int) character_count) { + return total_bytes_read; + } + + // Wait a set amount of time before trying again or the remaining time + // until the timeout if we are getting close + const uint64_t elapsed_us = hrt_elapsed_time(&start_time_us); + int64_t time_until_timeout = timeout_us - elapsed_us; + uint64_t time_to_sleep = 5000; + + if ((time_until_timeout >= 0) && + (time_until_timeout < (int64_t) time_to_sleep)) { + time_to_sleep = time_until_timeout; + } + + px4_usleep(time_to_sleep); + } + + return -1; +} + +ssize_t SerialImpl::write(const void *buffer, size_t buffer_size) +{ + if (!_open) { + PX4_ERR("Cannot write to serial device until it has been opened"); + return -1; + } + + int ret_write = qurt_uart_write(_serial_fd, (const char *) buffer, buffer_size); + + if (ret_write < 0) { + PX4_ERR("%s write error %d", _port, ret_write); + + } + + return ret_write; +} + +const char *SerialImpl::getPort() const +{ + return _port; +} + +uint32_t SerialImpl::getBaudrate() const +{ + return _baudrate; +} + +bool SerialImpl::setBaudrate(uint32_t baudrate) +{ + if (! validateBaudrate(baudrate)) { + PX4_ERR("Invalid baudrate: %u", baudrate); + return false; + } + + // check if already configured + if (baudrate == _baudrate) { + return true; + } + + _baudrate = baudrate; + + // process baud rate change now if port is already open + if (_open) { + return open(); + } + + return true; +} + +ByteSize SerialImpl::getBytesize() const +{ + return _bytesize; +} + +bool SerialImpl::setBytesize(ByteSize bytesize) +{ + return bytesize == ByteSize::EightBits; +} + +Parity SerialImpl::getParity() const +{ + return _parity; +} + +bool SerialImpl::setParity(Parity parity) +{ + return parity == Parity::None; +} + +StopBits SerialImpl::getStopbits() const +{ + return _stopbits; +} + +bool SerialImpl::setStopbits(StopBits stopbits) +{ + return stopbits == StopBits::One; +} + +FlowControl SerialImpl::getFlowcontrol() const +{ + return _flowcontrol; +} + +bool SerialImpl::setFlowcontrol(FlowControl flowcontrol) +{ + return flowcontrol == FlowControl::Disabled; +} + +} // namespace device diff --git a/platforms/qurt/src/px4/common/include/px4_platform/cpuload.h b/platforms/qurt/src/px4/common/include/px4_platform/cpuload.h new file mode 100644 index 000000000000..b4c397098a40 --- /dev/null +++ b/platforms/qurt/src/px4/common/include/px4_platform/cpuload.h @@ -0,0 +1,37 @@ +/**************************************************************************** + * + * Copyright (c) 2023 ModalAI, Inc. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +extern "C" float px4muorb_get_cpu_load(void); + diff --git a/platforms/qurt/src/px4/drv_hrt.cpp b/platforms/qurt/src/px4/drv_hrt.cpp index 31b274f3b272..2ef152083249 100644 --- a/platforms/qurt/src/px4/drv_hrt.cpp +++ b/platforms/qurt/src/px4/drv_hrt.cpp @@ -81,7 +81,7 @@ static void hrt_unlock() px4_sem_post(&_hrt_lock); } -int px4_clock_settime(clockid_t clk_id, struct timespec *tp) +int px4_clock_settime(clockid_t clk_id, const struct timespec *tp) { return 0; } @@ -116,16 +116,10 @@ hrt_abstime hrt_absolute_time() int hrt_set_absolute_time_offset(int32_t time_diff_us) { - dsp_offset = time_diff_us; + // dsp_offset = time_diff_us; return 0; } -hrt_abstime hrt_elapsed_time_atomic(const volatile hrt_abstime *then) -{ - hrt_abstime delta = hrt_absolute_time() - *then; - return delta; -} - void hrt_store_absolute_time(volatile hrt_abstime *t) { *t = hrt_absolute_time(); diff --git a/platforms/qurt/src/px4/tasks.cpp b/platforms/qurt/src/px4/tasks.cpp index 712c6c377c0d..529b234edc49 100644 --- a/platforms/qurt/src/px4/tasks.cpp +++ b/platforms/qurt/src/px4/tasks.cpp @@ -161,6 +161,9 @@ static px4_task_t px4_task_spawn_internal(const char *name, int priority, px4_ma return -1; } + char *charPointer = const_cast(name); + taskmap[task_index].argv[0] = charPointer; + for (i = 0; i < PX4_TASK_MAX_ARGC; i++) { if (i < taskmap[task_index].argc) { int argument_length = strlen(argv[i]); @@ -172,12 +175,13 @@ static px4_task_t px4_task_spawn_internal(const char *name, int priority, px4_ma } else { strcpy(taskmap[task_index].argv_storage[i], argv[i]); - taskmap[task_index].argv[i] = taskmap[task_index].argv_storage[i]; + taskmap[task_index].argv[i + 1] = taskmap[task_index].argv_storage[i]; } } else { // Must add NULL at end of argv - taskmap[task_index].argv[i] = nullptr; + taskmap[task_index].argv[i + 1] = nullptr; + taskmap[task_index].argc = i + 1; break; } } diff --git a/platforms/ros2/include/uORB/Publication.hpp b/platforms/ros2/include/uORB/Publication.hpp index 3b53424e0b35..b7f1d0768d0b 100644 --- a/platforms/ros2/include/uORB/Publication.hpp +++ b/platforms/ros2/include/uORB/Publication.hpp @@ -45,28 +45,10 @@ namespace uORB { -template class DefaultQueueSize -{ -private: - template - static constexpr uint8_t get_queue_size(decltype(T::ORB_QUEUE_LENGTH) *) - { - return T::ORB_QUEUE_LENGTH; - } - - template static constexpr uint8_t get_queue_size(...) - { - return 1; - } - -public: - static constexpr unsigned value = get_queue_size(nullptr); -}; - /** * uORB publication wrapper class */ -template::value> +template class Publication { public: diff --git a/src/drivers/actuators/modal_io/Kconfig b/src/drivers/actuators/modal_io/Kconfig deleted file mode 100644 index 742014f417e2..000000000000 --- a/src/drivers/actuators/modal_io/Kconfig +++ /dev/null @@ -1,5 +0,0 @@ -menuconfig DRIVERS_ACTUATORS_MODAL_IO - bool "modal_io" - default n - ---help--- - Enable support for modal_io diff --git a/src/drivers/actuators/modal_io/module.yaml b/src/drivers/actuators/modal_io/module.yaml deleted file mode 100644 index a7c79fae621b..000000000000 --- a/src/drivers/actuators/modal_io/module.yaml +++ /dev/null @@ -1,26 +0,0 @@ -module_name: MODAL IO Output -actuator_output: - show_subgroups_if: 'MODAL_IO_CONFIG>0' - config_parameters: - - param: 'MODAL_IO_CONFIG' - label: 'Configure' - function: 'enable' - - param: 'MODAL_IO_BAUD' - label: 'Bitrate' - - param: 'MODAL_IO_RPM_MIN' - label: 'RPM Min' - - param: 'MODAL_IO_RPM_MAX' - label: 'RPM Max' - - param: 'MODAL_IO_SDIR1' - label: 'ESC 1 Spin Direction' - - param: 'MODAL_IO_SDIR2' - label: 'ESC 2 Spin Direction' - - param: 'MODAL_IO_SDIR3' - label: 'ESC 3 Spin Direction' - - param: 'MODAL_IO_SDIR4' - label: 'ESC 4 Spin Direction' - output_groups: - - param_prefix: MODAL_IO - group_label: 'ESCs' - channel_label: 'ESC' - num_channels: 4 diff --git a/src/drivers/actuators/modal_io/CMakeLists.txt b/src/drivers/actuators/voxl_esc/CMakeLists.txt similarity index 93% rename from src/drivers/actuators/modal_io/CMakeLists.txt rename to src/drivers/actuators/voxl_esc/CMakeLists.txt index 06368a53e475..d588dc29a69d 100644 --- a/src/drivers/actuators/modal_io/CMakeLists.txt +++ b/src/drivers/actuators/voxl_esc/CMakeLists.txt @@ -32,20 +32,21 @@ ############################################################################ px4_add_module( - MODULE drivers__actuators__modal_io - MAIN modal_io + MODULE drivers__actuators__voxl_esc + MAIN voxl_esc SRCS crc16.c crc16.h - modal_io_serial.cpp - modal_io_serial.hpp - modal_io.cpp - modal_io.hpp + voxl_esc_serial.cpp + voxl_esc_serial.hpp + voxl_esc.cpp + voxl_esc.hpp qc_esc_packet_types.h qc_esc_packet.c qc_esc_packet.h DEPENDS + battery px4_work_queue mixer_module MODULE_CONFIG diff --git a/src/drivers/actuators/voxl_esc/Kconfig b/src/drivers/actuators/voxl_esc/Kconfig new file mode 100644 index 000000000000..5df8bb0bb4c6 --- /dev/null +++ b/src/drivers/actuators/voxl_esc/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_ACTUATORS_VOXL_ESC + bool "voxl_esc" + default n + ---help--- + Enable support for voxl_esc diff --git a/src/drivers/actuators/modal_io/crc16.c b/src/drivers/actuators/voxl_esc/crc16.c similarity index 100% rename from src/drivers/actuators/modal_io/crc16.c rename to src/drivers/actuators/voxl_esc/crc16.c diff --git a/src/drivers/actuators/modal_io/crc16.h b/src/drivers/actuators/voxl_esc/crc16.h similarity index 100% rename from src/drivers/actuators/modal_io/crc16.h rename to src/drivers/actuators/voxl_esc/crc16.h diff --git a/src/drivers/actuators/voxl_esc/module.yaml b/src/drivers/actuators/voxl_esc/module.yaml new file mode 100644 index 000000000000..1e227ae0b71d --- /dev/null +++ b/src/drivers/actuators/voxl_esc/module.yaml @@ -0,0 +1,41 @@ +module_name: VOXL ESC Output +actuator_output: + show_subgroups_if: 'VOXL_ESC_CONFIG>0' + config_parameters: + - param: 'VOXL_ESC_CONFIG' + label: 'Configure' + function: 'enable' + - param: 'VOXL_ESC_BAUD' + label: 'Bitrate' + - param: 'VOXL_ESC_RPM_MIN' + label: 'RPM Min' + - param: 'VOXL_ESC_RPM_MAX' + label: 'RPM Max' + - param: 'VOXL_ESC_SDIR1' + label: 'ESC 1 Spin Direction' + - param: 'VOXL_ESC_SDIR2' + label: 'ESC 2 Spin Direction' + - param: 'VOXL_ESC_SDIR3' + label: 'ESC 3 Spin Direction' + - param: 'VOXL_ESC_SDIR4' + label: 'ESC 4 Spin Direction' + output_groups: + - param_prefix: VOXL_ESC + group_label: 'ESCs' + channel_label: 'ESC' + num_channels: 4 + +parameters: + - group: ModalAI Custom Configuration + definitions: + MODALAI_CONFIG: + description: + short: Custom configuration for ModalAI drones + long: | + This can be set to indicate that drone behavior + needs to be changed to match a custom setting + type: int32 + reboot_required: true + num_instances: 1 + instance_start: 1 + default: 0 \ No newline at end of file diff --git a/src/drivers/actuators/modal_io/qc_esc_packet.c b/src/drivers/actuators/voxl_esc/qc_esc_packet.c similarity index 83% rename from src/drivers/actuators/modal_io/qc_esc_packet.c rename to src/drivers/actuators/voxl_esc/qc_esc_packet.c index 0dc8fab6649d..086622379cbf 100644 --- a/src/drivers/actuators/modal_io/qc_esc_packet.c +++ b/src/drivers/actuators/voxl_esc/qc_esc_packet.c @@ -119,36 +119,60 @@ int32_t qc_esc_create_pwm_packet4_fb(int16_t pwm0, int16_t pwm1, int16_t pwm2, i } -int32_t qc_esc_create_rpm_packet4(int16_t rpm0, int16_t rpm1, int16_t rpm2, int16_t rpm3, +int32_t qc_esc_create_rpm_packet4(int32_t rpm0, int32_t rpm1, int32_t rpm2, int32_t rpm3, uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3, - uint8_t *out, uint16_t out_size) + uint8_t *out, uint16_t out_size, uint8_t ext_rpm) { - return qc_esc_create_rpm_packet4_fb(rpm0, rpm1, rpm2, rpm3, led0, led1, led2, led3, -1, out, out_size); + return qc_esc_create_rpm_packet4_fb(rpm0, rpm1, rpm2, rpm3, led0, led1, led2, led3, -1, out, out_size, ext_rpm); } -int32_t qc_esc_create_rpm_packet4_fb(int16_t rpm0, int16_t rpm1, int16_t rpm2, int16_t rpm3, +int32_t qc_esc_create_rpm_packet4_fb(int32_t rpm0, int32_t rpm1, int32_t rpm2, int32_t rpm3, uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3, - int32_t fb_id, uint8_t *out, uint16_t out_size) + int32_t fb_id, uint8_t *out, uint16_t out_size, uint8_t ext_rpm) { - uint16_t data[5]; + int16_t data[5]; uint16_t leds = 0; + uint8_t cmd = ESC_PACKET_TYPE_RPM_CMD; + int32_t max = ext_rpm > 0 ? ESC_RPM_MAX_EXT : ESC_RPM_MAX; + int32_t min = ext_rpm > 0 ? ESC_RPM_MIN_EXT : ESC_RPM_MIN; - if (fb_id != -1) { fb_id = fb_id % 4; } + // Limit RPMs to prevent overflow when converting to int16_t - //least significant bit is used for feedback request - rpm0 &= ~(0x0001); rpm1 &= ~(0x0001); rpm2 &= ~(0x0001); rpm3 &= ~(0x0001); + if (rpm0 > max) { rpm0 = max; } if (rpm0 < min) { rpm0 = min; } - if (fb_id == 0) { rpm0 |= 0x0001; } if (fb_id == 1) { rpm1 |= 0x0001; } + if (rpm1 > max) { rpm1 = max; } if (rpm1 < min) { rpm1 = min; } - if (fb_id == 2) { rpm2 |= 0x0001; } if (fb_id == 3) { rpm3 |= 0x0001; } + if (rpm2 > max) { rpm2 = max; } if (rpm2 < min) { rpm2 = min; } - leds |= led0 & 0b00000111; + if (rpm3 > max) { rpm3 = max; } if (rpm3 < min) { rpm3 = min; } + + if (fb_id != -1) { fb_id = fb_id % 4; } + + leds |= led0 & 0b00000111; leds |= (led1 & 0b00000111) << 3; leds |= ((uint16_t)(led2 & 0b00000111)) << 6; leds |= ((uint16_t)(led3 & 0b00000111)) << 9; - data[0] = rpm0; data[1] = rpm1; data[2] = rpm2; data[3] = rpm3; data[4] = leds; - return qc_esc_create_packet(ESC_PACKET_TYPE_RPM_CMD, (uint8_t *) & (data[0]), 10, out, out_size); + if (ext_rpm > 0) { + cmd = ESC_PACKET_TYPE_RPM_DIV2_CMD; + data[0] = ((rpm0 / 4) * 2); + data[1] = ((rpm1 / 4) * 2); + data[2] = ((rpm2 / 4) * 2); + data[3] = ((rpm3 / 4) * 2); + data[4] = leds; + + } else { + data[0] = rpm0; data[1] = rpm1; data[2] = rpm2; data[3] = rpm3; data[4] = leds; + } + + //least significant bit is used for feedback request + data[0] &= ~(0x0001); data[1] &= ~(0x0001); data[2] &= ~(0x0001); data[3] &= ~(0x0001); + + if (fb_id == 0) { data[0] |= 0x0001; } if (fb_id == 1) { data[1] |= 0x0001; } + + if (fb_id == 2) { data[2] |= 0x0001; } if (fb_id == 3) { data[3] |= 0x0001; } + + return qc_esc_create_packet(cmd, (uint8_t *) & (data[0]), 10, out, out_size); } int32_t qc_esc_create_packet(uint8_t type, uint8_t *data, uint16_t size, uint8_t *out, uint16_t out_size) diff --git a/src/drivers/actuators/modal_io/qc_esc_packet.h b/src/drivers/actuators/voxl_esc/qc_esc_packet.h similarity index 91% rename from src/drivers/actuators/modal_io/qc_esc_packet.h rename to src/drivers/actuators/voxl_esc/qc_esc_packet.h index 8af2b6ea5483..88ecc8e9f9df 100644 --- a/src/drivers/actuators/modal_io/qc_esc_packet.h +++ b/src/drivers/actuators/voxl_esc/qc_esc_packet.h @@ -52,6 +52,11 @@ extern "C" { #define QC_ESC_LED_GREEN_ON 2 #define QC_ESC_LED_BLUE_ON 4 +// Define RPM command max and min values +#define ESC_RPM_MAX INT16_MAX-1 // 32k +#define ESC_RPM_MIN INT16_MIN+1 // -32k +#define ESC_RPM_MAX_EXT UINT16_MAX-5 // 65k +#define ESC_RPM_MIN_EXT -UINT16_MAX+5 // -65k // Header of the packet. Each packet must start with this header #define ESC_PACKET_HEADER 0xAF @@ -142,6 +147,18 @@ typedef struct { } __attribute__((__packed__)) QC_ESC_FB_RESPONSE_V2; +// Definition of the feedback response packet from ESC, which contains battery voltage and total current +typedef struct { + uint8_t header; + uint8_t length; + uint8_t type; + uint8_t id; //ESC Id (could be used as system ID elsewhere) + uint16_t voltage; //Input voltage (Millivolts) + int16_t current; //Total Current (8mA resolution) + uint16_t crc; +} __attribute__((__packed__)) QC_ESC_FB_POWER_STATUS; + + //------------------------------------------------------------------------- //Below are functions for generating packets that would be outgoing to ESCs //------------------------------------------------------------------------- @@ -197,15 +214,15 @@ int32_t qc_esc_create_pwm_packet4_fb(int16_t pwm0, int16_t pwm1, int16_t pwm2, i // Create a packet for sending closed-loop RPM command and LED command to 4 ESCs without requesting any feedback // Return value is the length of generated packet (if positive), otherwise error code -int32_t qc_esc_create_rpm_packet4(int16_t rpm0, int16_t rpm1, int16_t rpm2, int16_t rpm3, +int32_t qc_esc_create_rpm_packet4(int32_t rpm0, int32_t rpm1, int32_t rpm2, int32_t rpm3, uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3, - uint8_t *out, uint16_t out_size); + uint8_t *out, uint16_t out_size, uint8_t ext_rpm); -// Create a packet for sending closed-loop RPM command and LED command to 4 ESCs, also request feedback from one ESC (with id=fb_id) +// Create a packet for sending closed-loop RPM command (32766 or 65530 max RPM) and LED command to 4 ESCs, also request feedback from one ESC (with id=fb_id) // Return value is the length of generated packet (if positive), otherwise error code -int32_t qc_esc_create_rpm_packet4_fb(int16_t rpm0, int16_t rpm1, int16_t rpm2, int16_t rpm3, +int32_t qc_esc_create_rpm_packet4_fb(int32_t rpm0, int32_t rpm1, int32_t rpm2, int32_t rpm3, uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3, - int32_t fb_id, uint8_t *out, uint16_t out_size); + int32_t fb_id, uint8_t *out, uint16_t out_size, uint8_t ext_rpm); //------------------------------------------------------------------------- diff --git a/src/drivers/actuators/modal_io/qc_esc_packet_types.h b/src/drivers/actuators/voxl_esc/qc_esc_packet_types.h similarity index 97% rename from src/drivers/actuators/modal_io/qc_esc_packet_types.h rename to src/drivers/actuators/voxl_esc/qc_esc_packet_types.h index 2453d7dbf043..538a8859a7c1 100644 --- a/src/drivers/actuators/modal_io/qc_esc_packet_types.h +++ b/src/drivers/actuators/voxl_esc/qc_esc_packet_types.h @@ -46,6 +46,7 @@ #define ESC_PACKET_TYPE_SOUND_CMD 3 #define ESC_PACKET_TYPE_STEP_CMD 4 #define ESC_PACKET_TYPE_LED_CMD 5 +#define ESC_PACKET_TYPE_RPM_DIV2_CMD 7 #define ESC_PACKET_TYPE_RESET_CMD 10 #define ESC_PACKET_TYPE_SET_ID_CMD 11 #define ESC_PACKET_TYPE_SET_DIR_CMD 12 @@ -69,5 +70,6 @@ #define ESC_PACKET_TYPE_TUNE_CONFIG 114 #define ESC_PACKET_TYPE_FB_RESPONSE 128 #define ESC_PACKET_TYPE_VERSION_EXT_RESPONSE 131 +#define ESC_PACKET_TYPE_FB_POWER_STATUS 132 #endif diff --git a/src/drivers/actuators/modal_io/modal_io.cpp b/src/drivers/actuators/voxl_esc/voxl_esc.cpp similarity index 76% rename from src/drivers/actuators/modal_io/modal_io.cpp rename to src/drivers/actuators/voxl_esc/voxl_esc.cpp index 1e5a2bc08c43..88d070a7cf6c 100644 --- a/src/drivers/actuators/modal_io/modal_io.cpp +++ b/src/drivers/actuators/voxl_esc/voxl_esc.cpp @@ -35,33 +35,32 @@ #include -#include "modal_io.hpp" -#include "modal_io_serial.hpp" - -// utility for running on VOXL and using driver as a bridge -#define MODAL_IO_VOXL_BRIDGE_PORT "/dev/ttyS4" +#include "voxl_esc.hpp" +#include "voxl_esc_serial.hpp" // future use: #define MODALAI_PUBLISH_ESC_STATUS 0 const char *_device; -ModalIo::ModalIo() : - OutputModuleInterface(MODULE_NAME, px4::serial_port_to_wq(MODAL_IO_DEFAULT_PORT)), +VoxlEsc::VoxlEsc() : + OutputModuleInterface(MODULE_NAME, px4::serial_port_to_wq(VOXL_ESC_DEFAULT_PORT)), + _mixing_output{"VOXL_ESC", VOXL_ESC_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")) + _output_update_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": output update interval")), + _battery(1, nullptr, _battery_report_interval, battery_status_s::BATTERY_SOURCE_POWER_MODULE) { - _device = MODAL_IO_DEFAULT_PORT; + _device = VOXL_ESC_DEFAULT_PORT; _mixing_output.setAllFailsafeValues(0); _mixing_output.setAllDisarmedValues(0); _esc_status.timestamp = hrt_absolute_time(); _esc_status.counter = 0; - _esc_status.esc_count = MODAL_IO_OUTPUT_CHANNELS; + _esc_status.esc_count = VOXL_ESC_OUTPUT_CHANNELS; _esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_SERIAL; - for (unsigned i = 0; i < MODAL_IO_OUTPUT_CHANNELS; i++) { + for (unsigned i = 0; i < VOXL_ESC_OUTPUT_CHANNELS; i++) { _esc_status.esc[i].timestamp = 0; _esc_status.esc[i].esc_address = 0; _esc_status.esc[i].esc_rpm = 0; @@ -75,15 +74,12 @@ 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); _fb_idx = 0; } -ModalIo::~ModalIo() +VoxlEsc::~VoxlEsc() { _outputs_on = false; @@ -92,16 +88,11 @@ ModalIo::~ModalIo() _uart_port = nullptr; } - if (_uart_port_bridge) { - _uart_port_bridge->uart_close(); - _uart_port_bridge = nullptr; - } - perf_free(_cycle_perf); perf_free(_output_update_perf); } -int ModalIo::init() +int VoxlEsc::init() { /* Getting initial parameter values */ @@ -111,10 +102,15 @@ int ModalIo::init() return ret; } - _uart_port = new ModalIoSerial(); - _uart_port_bridge = new ModalIoSerial(); + _uart_port = new VoxlEscSerial(); memset(&_esc_chans, 0x00, sizeof(_esc_chans)); + for (int esc_id = 0; esc_id < VOXL_ESC_OUTPUT_CHANNELS; ++esc_id) { + _version_info[esc_id].sw_version = UINT16_MAX; + _version_info[esc_id].hw_version = UINT16_MAX; + _version_info[esc_id].id = esc_id; + } + //get_instance()->ScheduleOnInterval(10000); //100hz ScheduleNow(); @@ -122,81 +118,82 @@ int ModalIo::init() return 0; } -int ModalIo::load_params(modal_io_params_t *params, ch_assign_t *map) +int VoxlEsc::load_params(voxl_esc_params_t *params, ch_assign_t *map) { int ret = PX4_OK; // initialize out - for (int i = 0; i < MODAL_IO_OUTPUT_CHANNELS; i++) { + for (int i = 0; i < VOXL_ESC_OUTPUT_CHANNELS; i++) { params->function_map[i] = (int)OutputFunction::Disabled; params->direction_map[i] = 0; params->motor_map[i] = 0; } - param_get(param_find("MODAL_IO_CONFIG"), ¶ms->config); - param_get(param_find("MODAL_IO_MODE"), ¶ms->mode); - param_get(param_find("MODAL_IO_BAUD"), ¶ms->baud_rate); + param_get(param_find("VOXL_ESC_CONFIG"), ¶ms->config); + param_get(param_find("VOXL_ESC_MODE"), ¶ms->mode); + param_get(param_find("VOXL_ESC_BAUD"), ¶ms->baud_rate); - param_get(param_find("MODAL_IO_T_PERC"), ¶ms->turtle_motor_percent); - param_get(param_find("MODAL_IO_T_DEAD"), ¶ms->turtle_motor_deadband); - param_get(param_find("MODAL_IO_T_EXPO"), ¶ms->turtle_motor_expo); - param_get(param_find("MODAL_IO_T_MINF"), ¶ms->turtle_stick_minf); - param_get(param_find("MODAL_IO_T_COSP"), ¶ms->turtle_cosphi); + param_get(param_find("VOXL_ESC_T_PERC"), ¶ms->turtle_motor_percent); + param_get(param_find("VOXL_ESC_T_DEAD"), ¶ms->turtle_motor_deadband); + param_get(param_find("VOXL_ESC_T_EXPO"), ¶ms->turtle_motor_expo); + param_get(param_find("VOXL_ESC_T_MINF"), ¶ms->turtle_stick_minf); + param_get(param_find("VOXL_ESC_T_COSP"), ¶ms->turtle_cosphi); - param_get(param_find("MODAL_IO_FUNC1"), ¶ms->function_map[0]); - param_get(param_find("MODAL_IO_FUNC2"), ¶ms->function_map[1]); - param_get(param_find("MODAL_IO_FUNC3"), ¶ms->function_map[2]); - param_get(param_find("MODAL_IO_FUNC4"), ¶ms->function_map[3]); + param_get(param_find("VOXL_ESC_FUNC1"), ¶ms->function_map[0]); + param_get(param_find("VOXL_ESC_FUNC2"), ¶ms->function_map[1]); + param_get(param_find("VOXL_ESC_FUNC3"), ¶ms->function_map[2]); + param_get(param_find("VOXL_ESC_FUNC4"), ¶ms->function_map[3]); - param_get(param_find("MODAL_IO_SDIR1"), ¶ms->direction_map[0]); - param_get(param_find("MODAL_IO_SDIR2"), ¶ms->direction_map[1]); - param_get(param_find("MODAL_IO_SDIR3"), ¶ms->direction_map[2]); - param_get(param_find("MODAL_IO_SDIR4"), ¶ms->direction_map[3]); + param_get(param_find("VOXL_ESC_SDIR1"), ¶ms->direction_map[0]); + param_get(param_find("VOXL_ESC_SDIR2"), ¶ms->direction_map[1]); + param_get(param_find("VOXL_ESC_SDIR3"), ¶ms->direction_map[2]); + param_get(param_find("VOXL_ESC_SDIR4"), ¶ms->direction_map[3]); - param_get(param_find("MODAL_IO_RPM_MIN"), ¶ms->rpm_min); - param_get(param_find("MODAL_IO_RPM_MAX"), ¶ms->rpm_max); + param_get(param_find("VOXL_ESC_RPM_MIN"), ¶ms->rpm_min); + param_get(param_find("VOXL_ESC_RPM_MAX"), ¶ms->rpm_max); - param_get(param_find("MODAL_IO_VLOG"), ¶ms->verbose_logging); + param_get(param_find("VOXL_ESC_VLOG"), ¶ms->verbose_logging); + param_get(param_find("VOXL_ESC_PUB_BST"), ¶ms->publish_battery_status); if (params->rpm_min >= params->rpm_max) { - PX4_ERR("Invalid parameter MODAL_IO_RPM_MIN. Please verify parameters."); + PX4_ERR("Invalid parameter VOXL_ESC_RPM_MIN. Please verify parameters."); params->rpm_min = 0; ret = PX4_ERROR; } if (params->turtle_motor_percent < 0 || params->turtle_motor_percent > 100) { - PX4_ERR("Invalid parameter MODAL_IO_T_PERC. Please verify parameters."); + PX4_ERR("Invalid parameter VOXL_ESC_T_PERC. Please verify parameters."); params->turtle_motor_percent = 0; ret = PX4_ERROR; } if (params->turtle_motor_deadband < 0 || params->turtle_motor_deadband > 100) { - PX4_ERR("Invalid parameter MODAL_IO_T_DEAD. Please verify parameters."); + PX4_ERR("Invalid parameter VOXL_ESC_T_DEAD. Please verify parameters."); params->turtle_motor_deadband = 0; ret = PX4_ERROR; } if (params->turtle_motor_expo < 0 || params->turtle_motor_expo > 100) { - PX4_ERR("Invalid parameter MODAL_IO_T_EXPO. Please verify parameters."); + PX4_ERR("Invalid parameter VOXL_ESC_T_EXPO. Please verify parameters."); params->turtle_motor_expo = 0; ret = PX4_ERROR; } if (params->turtle_stick_minf < 0.0f || params->turtle_stick_minf > 100.0f) { - PX4_ERR("Invalid parameter MODAL_IO_T_MINF. Please verify parameters."); + PX4_ERR("Invalid parameter VOXL_ESC_T_MINF. Please verify parameters."); params->turtle_stick_minf = 0.0f; ret = PX4_ERROR; } if (params->turtle_cosphi < 0.0f || params->turtle_cosphi > 100.0f) { - PX4_ERR("Invalid parameter MODAL_IO_T_COSP. Please verify parameters."); + PX4_ERR("Invalid parameter VOXL_ESC_T_COSP. Please verify parameters."); params->turtle_cosphi = 0.0f; ret = PX4_ERROR; } - for (int i = 0; i < MODAL_IO_OUTPUT_CHANNELS; i++) { + for (int i = 0; i < VOXL_ESC_OUTPUT_CHANNELS; i++) { if (params->function_map[i] < (int)OutputFunction::Motor1 || params->function_map[i] > (int)OutputFunction::Motor4) { - PX4_ERR("Invalid parameter MODAL_IO_FUNCX. Only supports motors 1-4. Please verify parameters."); + PX4_ERR("Invalid parameter VOXL_ESC_FUNCX. Only supports motors 1-4. Please verify parameters."); params->function_map[i] = 0; ret = PX4_ERROR; @@ -210,11 +207,11 @@ int ModalIo::load_params(modal_io_params_t *params, ch_assign_t *map) } } - for (int i = 0; i < MODAL_IO_OUTPUT_CHANNELS; i++) { - if (params->motor_map[i] == MODAL_IO_OUTPUT_DISABLED || - params->motor_map[i] < -(MODAL_IO_OUTPUT_CHANNELS) || - params->motor_map[i] > MODAL_IO_OUTPUT_CHANNELS) { - PX4_ERR("Invalid parameter MODAL_IO_MOTORX. Please verify parameters."); + for (int i = 0; i < VOXL_ESC_OUTPUT_CHANNELS; i++) { + if (params->motor_map[i] == VOXL_ESC_OUTPUT_DISABLED || + params->motor_map[i] < -(VOXL_ESC_OUTPUT_CHANNELS) || + params->motor_map[i] > VOXL_ESC_OUTPUT_CHANNELS) { + PX4_ERR("Invalid parameter VOXL_ESC_MOTORX. Please verify parameters."); params->motor_map[i] = 0; ret = PX4_ERROR; } @@ -227,7 +224,7 @@ int ModalIo::load_params(modal_io_params_t *params, ch_assign_t *map) return ret; } -int ModalIo::task_spawn(int argc, char *argv[]) +int VoxlEsc::task_spawn(int argc, char *argv[]) { int myoptind = 0; int ch; @@ -244,7 +241,7 @@ int ModalIo::task_spawn(int argc, char *argv[]) } } - ModalIo *instance = new ModalIo(); + VoxlEsc *instance = new VoxlEsc(); if (instance) { _object.store(instance); @@ -267,14 +264,31 @@ int ModalIo::task_spawn(int argc, char *argv[]) return PX4_ERROR; } -int ModalIo::flush_uart_rx() +int VoxlEsc::flush_uart_rx() { while (_uart_port->uart_read(_read_buf, sizeof(_read_buf)) > 0) {} return 0; } -int ModalIo::read_response(Command *out_cmd) +bool VoxlEsc::check_versions_updated() +{ + for (int esc_id = 0; esc_id < VOXL_ESC_OUTPUT_CHANNELS; ++esc_id) { + if (_version_info[esc_id].sw_version == UINT16_MAX) { return false; } + } + + // PX4_INFO("Got all ESC Version info!"); + _extended_rpm = true; + _need_version_info = false; + + for (int esc_id = 0; esc_id < VOXL_ESC_OUTPUT_CHANNELS; ++esc_id) { + if (_version_info[esc_id].sw_version < VOXL_ESC_EXT_RPM) { _extended_rpm = false; } + } + + return true; +} + +int VoxlEsc::read_response(Command *out_cmd) { px4_usleep(_current_cmd.resp_delay_us); @@ -297,7 +311,7 @@ int ModalIo::read_response(Command *out_cmd) return 0; } -int ModalIo::parse_response(uint8_t *buf, uint8_t len, bool print_feedback) +int VoxlEsc::parse_response(uint8_t *buf, uint8_t len, bool print_feedback) { hrt_abstime tnow = hrt_absolute_time(); @@ -311,13 +325,13 @@ int ModalIo::parse_response(uint8_t *buf, uint8_t len, bool print_feedback) uint8_t packet_size = qc_esc_packet_get_size(&_fb_packet); if (packet_type == ESC_PACKET_TYPE_FB_RESPONSE && packet_size == sizeof(QC_ESC_FB_RESPONSE_V2)) { - //PX4_INFO("Got feedback V2 packet!"); + // PX4_INFO("Got feedback V2 packet!"); QC_ESC_FB_RESPONSE_V2 fb; memcpy(&fb, _fb_packet.buffer, packet_size); uint32_t id = (fb.id_state & 0xF0) >> 4; //ID of the ESC based on hardware address - if (id < MODAL_IO_OUTPUT_CHANNELS) { + if (id < VOXL_ESC_OUTPUT_CHANNELS) { int motor_idx = _output_map[id].number - 1; // mapped motor id.. user defined mapping is 1-4, array is 0-3 @@ -335,9 +349,9 @@ int ModalIo::parse_response(uint8_t *buf, uint8_t len, bool print_feedback) _esc_chans[id].power_applied = fb.power; _esc_chans[id].state = fb.id_state & 0x0F; _esc_chans[id].cmd_counter = fb.cmd_counter; - _esc_chans[id].voltage = fb.voltage * 0.001; - _esc_chans[id].current = fb.current * 0.008; - _esc_chans[id].temperature = fb.temperature * 0.01; + _esc_chans[id].voltage = fb.voltage * 0.001f; + _esc_chans[id].current = fb.current * 0.008f; + _esc_chans[id].temperature = fb.temperature * 0.01f; _esc_chans[id].feedback_time = tnow; // also update our internal report for logging @@ -382,6 +396,13 @@ int ModalIo::parse_response(uint8_t *buf, uint8_t len, bool print_feedback) else if (packet_type == ESC_PACKET_TYPE_VERSION_RESPONSE && packet_size == sizeof(QC_ESC_VERSION_INFO)) { QC_ESC_VERSION_INFO ver; memcpy(&ver, _fb_packet.buffer, packet_size); + + if (_need_version_info) { + memcpy(&_version_info[ver.id], &ver, sizeof(QC_ESC_VERSION_INFO)); + check_versions_updated(); + break; + } + PX4_INFO("ESC ID: %i", ver.id); PX4_INFO("HW Version: %i", ver.hw_version); PX4_INFO("SW Version: %i", ver.sw_version); @@ -400,83 +421,60 @@ int ModalIo::parse_response(uint8_t *buf, uint8_t len, bool print_feedback) PX4_INFO("\tFirmware : version %4d, hash %.12s", ver.sw_version, ver.firmware_git_version); PX4_INFO("\tBootloader : version %4d, hash %.12s", ver.bootloader_version, ver.bootloader_git_version); + + } else if (packet_type == ESC_PACKET_TYPE_FB_POWER_STATUS && packet_size == sizeof(QC_ESC_FB_POWER_STATUS)) { + QC_ESC_FB_POWER_STATUS packet; + memcpy(&packet, _fb_packet.buffer, packet_size); + + float voltage = packet.voltage * 0.001f; // Voltage is reported at 1 mV resolution + float current = packet.current * 0.008f; // Total current is reported at 8mA resolution + + // Limit the frequency of battery status reports + if (_parameters.publish_battery_status) { + _battery.setConnected(true); + _battery.updateVoltage(voltage); + _battery.updateCurrent(current); + + hrt_abstime current_time = hrt_absolute_time(); + + if ((current_time - _last_battery_report_time) >= _battery_report_interval) { + _last_battery_report_time = current_time; + _battery.updateAndPublishBatteryStatus(current_time); + } + } + } } else { //parser error switch (ret) { case ESC_ERROR_BAD_CHECKSUM: _rx_crc_error_count++; - //PX4_INFO("BAD ESC packet checksum"); + // PX4_INFO("BAD ESC packet checksum"); break; case ESC_ERROR_BAD_LENGTH: - //PX4_INFO("BAD ESC packet length"); + // PX4_INFO("BAD ESC packet length"); break; } } } - /* - if (len < 4 || buf[0] != ESC_PACKET_HEADER) { - return -1; - } - - switch (buf[2]) { - case ESC_PACKET_TYPE_VERSION_RESPONSE: - if (len != sizeof(QC_ESC_VERSION_INFO)) { - return -1; - - } else { - QC_ESC_VERSION_INFO ver; - memcpy(&ver, buf, len); - PX4_INFO("ESC ID: %i", ver.id); - PX4_INFO("HW Version: %i", ver.hw_version); - PX4_INFO("SW Version: %i", ver.sw_version); - PX4_INFO("Unique ID: %i", ver.unique_id); - } - - break; - - case ESC_PACKET_TYPE_FB_RESPONSE: - if (len != sizeof(QC_ESC_FB_RESPONSE)) { - return -1; - - } else { - QC_ESC_FB_RESPONSE fb; - memcpy(&fb, buf, len); - uint8_t id = (fb.state & 0xF0) >> 4; - - if (id < MODAL_IO_OUTPUT_CHANNELS) { - _esc_chans[id].rate_meas = fb.rpm; - _esc_chans[id].state = fb.state & 0x0F; - _esc_chans[id].cmd_counter = fb.cmd_counter; - _esc_chans[id].voltage = 9.0 + fb.voltage / 34.0; - } - } - - break; - - default: - return -1; - } - */ - return 0; } -int ModalIo::check_for_esc_timeout() +int VoxlEsc::check_for_esc_timeout() { hrt_abstime tnow = hrt_absolute_time(); - for (int i = 0; i < MODAL_IO_OUTPUT_CHANNELS; i++) { + for (int i = 0; i < VOXL_ESC_OUTPUT_CHANNELS; i++) { // PX4 motor indexed user defined mapping is 1-4, we want to use in bitmask (0-3) uint8_t motor_idx = _output_map[i].number - 1; - if (motor_idx < MODAL_IO_OUTPUT_CHANNELS) { + if (motor_idx < VOXL_ESC_OUTPUT_CHANNELS) { // we are using PX4 motor index in the bitmask if (_esc_status.esc_online_flags & (1 << motor_idx)) { // using index i here for esc_chans enumeration stored in ESC ID order - if ((tnow - _esc_chans[i].feedback_time) > MODAL_IO_DISCONNECT_TIMEOUT_US) { + if ((tnow - _esc_chans[i].feedback_time) > VOXL_ESC_DISCONNECT_TIMEOUT_US) { // stale data, assume offline and clear armed _esc_status.esc_online_flags &= ~(1 << motor_idx); _esc_status.esc_armed_flags &= ~(1 << motor_idx); @@ -489,7 +487,7 @@ int ModalIo::check_for_esc_timeout() } -int ModalIo::send_cmd_thread_safe(Command *cmd) +int VoxlEsc::send_cmd_thread_safe(Command *cmd) { cmd->id = _cmd_id++; _pending_cmd.store(cmd); @@ -504,7 +502,7 @@ int ModalIo::send_cmd_thread_safe(Command *cmd) -int ModalIo::custom_command(int argc, char *argv[]) +int VoxlEsc::custom_command(int argc, char *argv[]) { int myoptind = 0; int ch; @@ -530,7 +528,7 @@ int ModalIo::custom_command(int argc, char *argv[]) /* start the FMU if not running */ if (!strcmp(verb, "start")) { if (!is_running()) { - return ModalIo::task_spawn(argc, argv); + return VoxlEsc::task_spawn(argc, argv); } } @@ -593,7 +591,7 @@ int ModalIo::custom_command(int argc, char *argv[]) } if (!strcmp(verb, "reset")) { - if (esc_id < MODAL_IO_OUTPUT_CHANNELS) { + if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) { PX4_INFO("Reset ESC: %i", esc_id); cmd.len = qc_esc_create_reset_packet(esc_id, cmd.buf, sizeof(cmd.buf)); cmd.response = false; @@ -605,7 +603,7 @@ int ModalIo::custom_command(int argc, char *argv[]) } } else if (!strcmp(verb, "version")) { - if (esc_id < MODAL_IO_OUTPUT_CHANNELS) { + if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) { PX4_INFO("Request version for ESC: %i", esc_id); cmd.len = qc_esc_create_version_request_packet(esc_id, cmd.buf, sizeof(cmd.buf)); cmd.response = true; @@ -618,7 +616,7 @@ int ModalIo::custom_command(int argc, char *argv[]) } } else if (!strcmp(verb, "version-ext")) { - if (esc_id < MODAL_IO_OUTPUT_CHANNELS) { + if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) { PX4_INFO("Request extended version for ESC: %i", esc_id); cmd.len = qc_esc_create_extended_version_request_packet(esc_id, cmd.buf, sizeof(cmd.buf)); cmd.response = true; @@ -631,7 +629,7 @@ int ModalIo::custom_command(int argc, char *argv[]) } } else if (!strcmp(verb, "tone")) { - if (esc_id < MODAL_IO_OUTPUT_CHANNELS) { + if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) { PX4_INFO("Request tone for ESC mask: %i", esc_id); cmd.len = qc_esc_create_sound_packet(period, duration, power, esc_id, cmd.buf, sizeof(cmd.buf)); cmd.response = false; @@ -660,12 +658,12 @@ int ModalIo::custom_command(int argc, char *argv[]) } } else if (!strcmp(verb, "rpm")) { - if (esc_id < MODAL_IO_OUTPUT_CHANNELS) { + if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) { PX4_INFO("Request RPM for ESC ID: %i - RPM: %i", esc_id, rate); - int16_t rate_req[MODAL_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0}; + int16_t rate_req[VOXL_ESC_OUTPUT_CHANNELS] = {0, 0, 0, 0}; uint8_t id_fb = 0; - if (esc_id == 0xFF) { + if (esc_id == 0xFF) { //WARNING: this condition is not possible due to check 'if (esc_id < VOXL_ESC_OUTPUT_CHANNELS)'. rate_req[0] = rate; rate_req[1] = rate; rate_req[2] = rate; @@ -686,7 +684,8 @@ int ModalIo::custom_command(int argc, char *argv[]) 0, id_fb, cmd.buf, - sizeof(cmd.buf)); + sizeof(cmd.buf), + get_instance()->_extended_rpm); cmd.response = true; cmd.repeats = repeat_count; @@ -705,12 +704,12 @@ int ModalIo::custom_command(int argc, char *argv[]) } } else if (!strcmp(verb, "pwm")) { - if (esc_id < MODAL_IO_OUTPUT_CHANNELS) { + if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) { PX4_INFO("Request PWM for ESC ID: %i - PWM: %i", esc_id, rate); - int16_t rate_req[MODAL_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0}; + int16_t rate_req[VOXL_ESC_OUTPUT_CHANNELS] = {0, 0, 0, 0}; uint8_t id_fb = 0; - if (esc_id == 0xFF) { + if (esc_id == 0xFF) { //WARNING: this condition is not possible due to check 'if (esc_id < VOXL_ESC_OUTPUT_CHANNELS)'. rate_req[0] = rate; rate_req[1] = rate; rate_req[2] = rate; @@ -745,7 +744,7 @@ int ModalIo::custom_command(int argc, char *argv[]) return get_instance()->send_cmd_thread_safe(&cmd); } else { - print_usage("Invalid ESC mask, use 1-15"); + print_usage("Invalid ESC ID, use 0-3"); return 0; } } @@ -753,7 +752,7 @@ int ModalIo::custom_command(int argc, char *argv[]) return print_usage("unknown command"); } -int ModalIo::update_params() +int VoxlEsc::update_params() { int ret = PX4_ERROR; @@ -772,7 +771,7 @@ int ModalIo::update_params() return ret; } -void ModalIo::update_leds(vehicle_control_mode_s mode, led_control_s control) +void VoxlEsc::update_leds(vehicle_control_mode_s mode, led_control_s control) { int i = 0; uint8_t led_mask = _led_rsc.led_mask; @@ -872,12 +871,12 @@ void ModalIo::update_leds(vehicle_control_mode_s mode, led_control_s control) } } - for (i = 0; i < MODAL_IO_OUTPUT_CHANNELS; i++) { + for (i = 0; i < VOXL_ESC_OUTPUT_CHANNELS; i++) { _esc_chans[i].led = led_mask; } } -void ModalIo::mix_turtle_mode(uint16_t outputs[MAX_ACTUATORS]) +void VoxlEsc::mix_turtle_mode(uint16_t outputs[MAX_ACTUATORS]) { bool use_pitch = true; bool use_roll = true; @@ -1052,10 +1051,13 @@ void ModalIo::mix_turtle_mode(uint16_t outputs[MAX_ACTUATORS]) } /* OutputModuleInterface */ -bool ModalIo::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], +bool VoxlEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { - if (num_outputs != MODAL_IO_OUTPUT_CHANNELS) { + //in Run() we call _mixing_output.update(), which calls MixingOutput::limitAndUpdateOutputs which calls _interface.updateOutputs (this function) + //So, if Run() is blocked by a custom command, this function will not be called until Run is running again + + if (num_outputs != VOXL_ESC_OUTPUT_CHANNELS) { return false; } @@ -1064,11 +1066,18 @@ bool ModalIo::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], mix_turtle_mode(outputs); } - for (int i = 0; i < MODAL_IO_OUTPUT_CHANNELS; i++) { + for (int i = 0; i < VOXL_ESC_OUTPUT_CHANNELS; i++) { if (!_outputs_on || stop_motors) { _esc_chans[i].rate_req = 0; } else { + if (_extended_rpm) { + if (outputs[i] > VOXL_ESC_RPM_MAX_EXT) { outputs[i] = VOXL_ESC_RPM_MAX_EXT; } + + } else { + if (outputs[i] > VOXL_ESC_RPM_MAX) { outputs[i] = VOXL_ESC_RPM_MAX; } + } + if (!_turtle_mode_en) { _esc_chans[i].rate_req = outputs[i] * _output_map[i].direction; @@ -1079,6 +1088,7 @@ bool ModalIo::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], } } + Command cmd; cmd.len = qc_esc_create_rpm_packet4_fb(_esc_chans[0].rate_req, _esc_chans[1].rate_req, @@ -1090,24 +1100,24 @@ bool ModalIo::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], _esc_chans[3].led, _fb_idx, cmd.buf, - sizeof(cmd.buf)); - + sizeof(cmd.buf), + _extended_rpm); if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { PX4_ERR("Failed to send packet"); return false; } - // round robin - _fb_idx = (_fb_idx + 1) % MODAL_IO_OUTPUT_CHANNELS; + // increment ESC id from which to request feedback in round robin order + _fb_idx = (_fb_idx + 1) % VOXL_ESC_OUTPUT_CHANNELS; /* - * Here we parse the feedback response. Rarely the packet is mangled - * but this means we simply miss a feedback response and will come back - * around in roughly 8ms for another... so don't freak out and keep on - * trucking I say + * Here we read and parse response from ESCs. Since the latest command has just been sent out, + * the response packet we may read here is probabaly from previous iteration, but it is totally ok. + * uart_read is non-blocking and we will just parse whatever bytes came in up until this point */ + int res = _uart_port->uart_read(_read_buf, sizeof(_read_buf)); if (res > 0) { @@ -1135,13 +1145,29 @@ bool ModalIo::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], _esc_status_pub.publish(_esc_status); + // If any extra external modal io data has been received then + // send it over as well + while (_voxl2_io_data_sub.updated()) { + buffer128_s io_data{}; + _voxl2_io_data_sub.copy(&io_data); + + // PX4_INFO("Got Modal IO data: %u bytes", io_data.len); + // PX4_INFO(" 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x", + // io_data.data[0], io_data.data[1], io_data.data[2], io_data.data[3], + // io_data.data[4], io_data.data[5], io_data.data[6], io_data.data[7]); + if (_uart_port->uart_write(io_data.data, io_data.len) != io_data.len) { + PX4_ERR("Failed to send modal io data to esc"); + return false; + } + } + perf_count(_output_update_perf); return true; } -void ModalIo::Run() +void VoxlEsc::Run() { if (should_exit()) { ScheduleClear(); @@ -1164,23 +1190,22 @@ void ModalIo::Run() } } - /* - for (int ii=0; ii<9; ii++) - { - const char * test_str = "Hello World!"; - _uart_port_bridge->uart_write((char*)test_str,12); - px4_usleep(10000); - } - */ - /* - uint8_t echo_buf[16]; - int bytes_read = _uart_port_bridge->uart_read(echo_buf,sizeof(echo_buf)); - if (bytes_read > 0) - _uart_port_bridge->uart_write(echo_buf,bytes_read); - */ + /* Get ESC FW version info */ + if (_need_version_info) { + for (uint8_t esc_id = 0; esc_id < VOXL_ESC_OUTPUT_CHANNELS; ++esc_id) { + Command cmd; + cmd.len = qc_esc_create_version_request_packet(esc_id, cmd.buf, sizeof(cmd.buf)); + + if (_uart_port->uart_write(cmd.buf, cmd.len) == cmd.len) { + if (read_response(&_current_cmd) != 0) { PX4_ERR("Failed to parse version request response packet!"); } + } else { + PX4_ERR("Failed to send version request packet!"); + } + } + } - _mixing_output.update(); + _mixing_output.update(); //calls MixingOutput::limitAndUpdateOutputs which calls updateOutputs in this module /* update output status if armed */ _outputs_on = _mixing_output.armed().armed; @@ -1222,16 +1247,16 @@ void ModalIo::Run() if (!_outputs_on) { - float setpoint = MODAL_IO_MODE_DISABLED_SETPOINT; + float setpoint = VOXL_ESC_MODE_DISABLED_SETPOINT; - if (_parameters.mode == MODAL_IO_MODE_TURTLE_AUX1) { + if (_parameters.mode == VOXL_ESC_MODE_TURTLE_AUX1) { setpoint = _manual_control_setpoint.aux1; - } else if (_parameters.mode == MODAL_IO_MODE_TURTLE_AUX2) { + } else if (_parameters.mode == VOXL_ESC_MODE_TURTLE_AUX2) { setpoint = _manual_control_setpoint.aux2; } - if (setpoint > MODAL_IO_MODE_THRESHOLD) { + if (setpoint > VOXL_ESC_MODE_THRESHOLD) { _turtle_mode_en = true; } else { @@ -1240,68 +1265,6 @@ void ModalIo::Run() } } - if (_parameters.mode == MODAL_IO_MODE_UART_BRIDGE) { - if (!_uart_port_bridge->is_open()) { - if (_uart_port_bridge->uart_open(MODAL_IO_VOXL_BRIDGE_PORT, 230400) == PX4_OK) { - PX4_INFO("Opened UART ESC Bridge device"); - - } else { - PX4_ERR("Failed openening UART ESC Bridge device"); - return; - } - } - - //uart passthrough test code - //run 9 times because i just don't know how to change update rate of the module from 10hz to 100hz.. - for (int ii = 0; ii < 9; ii++) { - uint8_t uart_buf[128]; - int bytes_read = _uart_port_bridge->uart_read(uart_buf, sizeof(uart_buf)); - - if (bytes_read > 0) { - _uart_port->uart_write(uart_buf, bytes_read); - - for (int i = 0; i < bytes_read; i++) { - int16_t ret = qc_esc_packet_process_char(uart_buf[i], &_uart_bridge_packet); - - if (ret > 0) { - //PX4_INFO("got packet of length %i",ret); - uint8_t packet_type = qc_esc_packet_get_type(&_uart_bridge_packet); - - //uint8_t packet_size = qc_esc_packet_get_size(&_uart_bridge_packet); - //if we received a command for ESC to reset, most likely firmware update is coming, switch to bootloader baud rate - if (packet_type == ESC_PACKET_TYPE_RESET_CMD) { - int bootloader_baud_rate = 230400; - - if (_uart_port->uart_get_baud() != bootloader_baud_rate) { - px4_usleep(5000); - _uart_port->uart_set_baud(bootloader_baud_rate); - } - - } else { - if (_uart_port->uart_get_baud() != _parameters.baud_rate) { - px4_usleep(5000); - _uart_port->uart_set_baud(_parameters.baud_rate); //restore normal baud rate - } - } - } - } - } - - bytes_read = _uart_port->uart_read(uart_buf, sizeof(uart_buf)); - - if (bytes_read > 0) { - _uart_port_bridge->uart_write(uart_buf, bytes_read); - } - - px4_usleep(10000); - } - } - - } else { - if (_uart_port_bridge->is_open()) { - PX4_INFO("Closed UART ESC Bridge device"); - _uart_port_bridge->uart_close(); - } } if (!_outputs_on) { @@ -1364,7 +1327,7 @@ void ModalIo::Run() } -int ModalIo::print_usage(const char *reason) +int VoxlEsc::print_usage(const char *reason) { if (reason) { PX4_WARN("%s\n", reason); @@ -1384,7 +1347,7 @@ It is typically started with: )DESCR_STR"); - PRINT_MODULE_USAGE_NAME("modal_io", "driver"); + PRINT_MODULE_USAGE_NAME("voxl_esc", "driver"); PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the task"); PRINT_MODULE_USAGE_COMMAND_DESCR("reset", "Send reset request to ESC"); @@ -1422,7 +1385,7 @@ It is typically started with: return 0; } -int ModalIo::print_status() +int VoxlEsc::print_status() { PX4_INFO("Max update rate: %i Hz", _current_update_rate); PX4_INFO("Outputs on: %s", _outputs_on ? "yes" : "no"); @@ -1431,31 +1394,31 @@ int ModalIo::print_status() PX4_INFO(""); - PX4_INFO("Params: MODAL_IO_CONFIG: %" PRId32, _parameters.config); - PX4_INFO("Params: MODAL_IO_BAUD: %" PRId32, _parameters.baud_rate); + PX4_INFO("Params: VOXL_ESC_CONFIG: %" PRId32, _parameters.config); + PX4_INFO("Params: VOXL_ESC_BAUD: %" PRId32, _parameters.baud_rate); - PX4_INFO("Params: MODAL_IO_FUNC1: %" PRId32, _parameters.function_map[0]); - PX4_INFO("Params: MODAL_IO_FUNC2: %" PRId32, _parameters.function_map[1]); - PX4_INFO("Params: MODAL_IO_FUNC3: %" PRId32, _parameters.function_map[2]); - PX4_INFO("Params: MODAL_IO_FUNC4: %" PRId32, _parameters.function_map[3]); + PX4_INFO("Params: VOXL_ESC_FUNC1: %" PRId32, _parameters.function_map[0]); + PX4_INFO("Params: VOXL_ESC_FUNC2: %" PRId32, _parameters.function_map[1]); + PX4_INFO("Params: VOXL_ESC_FUNC3: %" PRId32, _parameters.function_map[2]); + PX4_INFO("Params: VOXL_ESC_FUNC4: %" PRId32, _parameters.function_map[3]); - PX4_INFO("Params: MODAL_IO_SDIR1: %" PRId32, _parameters.direction_map[0]); - PX4_INFO("Params: MODAL_IO_SDIR2: %" PRId32, _parameters.direction_map[1]); - PX4_INFO("Params: MODAL_IO_SDIR3: %" PRId32, _parameters.direction_map[2]); - PX4_INFO("Params: MODAL_IO_SDIR4: %" PRId32, _parameters.direction_map[3]); + PX4_INFO("Params: VOXL_ESC_SDIR1: %" PRId32, _parameters.direction_map[0]); + PX4_INFO("Params: VOXL_ESC_SDIR2: %" PRId32, _parameters.direction_map[1]); + PX4_INFO("Params: VOXL_ESC_SDIR3: %" PRId32, _parameters.direction_map[2]); + PX4_INFO("Params: VOXL_ESC_SDIR4: %" PRId32, _parameters.direction_map[3]); - PX4_INFO("Params: MODAL_IO_RPM_MIN: %" PRId32, _parameters.rpm_min); - PX4_INFO("Params: MODAL_IO_RPM_MAX: %" PRId32, _parameters.rpm_max); + PX4_INFO("Params: VOXL_ESC_RPM_MIN: %" PRId32, _parameters.rpm_min); + PX4_INFO("Params: VOXL_ESC_RPM_MAX: %" PRId32, _parameters.rpm_max); PX4_INFO(""); - for( int i = 0; i < MODAL_IO_OUTPUT_CHANNELS; i++){ + for( int i = 0; i < VOXL_ESC_OUTPUT_CHANNELS; i++){ PX4_INFO("-- ID: %i", i); PX4_INFO(" Motor: %i", _output_map[i].number); PX4_INFO(" Direction: %i", _output_map[i].direction); PX4_INFO(" State: %i", _esc_chans[i].state); - PX4_INFO(" Requested: %i RPM", _esc_chans[i].rate_req); - PX4_INFO(" Measured: %i RPM", _esc_chans[i].rate_meas); + PX4_INFO(" Requested: %" PRIi32 " RPM", _esc_chans[i].rate_req); + PX4_INFO(" Measured: %" PRIi32 " RPM", _esc_chans[i].rate_meas); PX4_INFO(" Command Counter: %i", _esc_chans[i].cmd_counter); PX4_INFO(" Voltage: %f VDC", (double)_esc_chans[i].voltage); PX4_INFO(""); @@ -1469,9 +1432,9 @@ int ModalIo::print_status() return 0; } -extern "C" __EXPORT int modal_io_main(int argc, char *argv[]); +extern "C" __EXPORT int voxl_esc_main(int argc, char *argv[]); -int modal_io_main(int argc, char *argv[]) +int voxl_esc_main(int argc, char *argv[]) { - return ModalIo::main(argc, argv); + return VoxlEsc::main(argc, argv); } diff --git a/src/drivers/actuators/modal_io/modal_io.hpp b/src/drivers/actuators/voxl_esc/voxl_esc.hpp similarity index 68% rename from src/drivers/actuators/modal_io/modal_io.hpp rename to src/drivers/actuators/voxl_esc/voxl_esc.hpp index 196728aab7f1..8cc84435fae4 100644 --- a/src/drivers/actuators/modal_io/modal_io.hpp +++ b/src/drivers/actuators/voxl_esc/voxl_esc.hpp @@ -41,23 +41,26 @@ #include #include +#include + #include #include #include #include #include #include +#include -#include "modal_io_serial.hpp" +#include "voxl_esc_serial.hpp" #include "qc_esc_packet.h" #include "qc_esc_packet_types.h" -class ModalIo : public ModuleBase, public OutputModuleInterface +class VoxlEsc : public ModuleBase, public OutputModuleInterface { public: - ModalIo(); - virtual ~ModalIo(); + VoxlEsc(); + virtual ~VoxlEsc(); /** @see ModuleBase */ static int task_spawn(int argc, char *argv[]); @@ -100,56 +103,61 @@ class ModalIo : public ModuleBase, public OutputModuleInterface int send_cmd_thread_safe(Command *cmd); private: - static constexpr uint32_t MODAL_IO_UART_CONFIG = 1; - static constexpr uint32_t MODAL_IO_DEFAULT_BAUD = 250000; - static constexpr uint16_t MODAL_IO_OUTPUT_CHANNELS = 4; - static constexpr uint16_t MODAL_IO_OUTPUT_DISABLED = 0; + static constexpr uint32_t VOXL_ESC_UART_CONFIG = 1; + static constexpr uint32_t VOXL_ESC_DEFAULT_BAUD = 250000; + static constexpr uint16_t VOXL_ESC_OUTPUT_CHANNELS = 4; + static constexpr uint16_t VOXL_ESC_OUTPUT_DISABLED = 0; - static constexpr uint32_t MODAL_IO_WRITE_WAIT_US = 200; - static constexpr uint32_t MODAL_IO_DISCONNECT_TIMEOUT_US = 500000; + static constexpr uint32_t VOXL_ESC_WRITE_WAIT_US = 200; + static constexpr uint32_t VOXL_ESC_DISCONNECT_TIMEOUT_US = 500000; static constexpr uint16_t DISARMED_VALUE = 0; - static constexpr uint16_t MODAL_IO_PWM_MIN = 0; - static constexpr uint16_t MODAL_IO_PWM_MAX = 800; - static constexpr uint16_t MODAL_IO_DEFAULT_RPM_MIN = 5000; - static constexpr uint16_t MODAL_IO_DEFAULT_RPM_MAX = 17000; + static constexpr uint16_t VOXL_ESC_PWM_MIN = 0; + static constexpr uint16_t VOXL_ESC_PWM_MAX = 800; + static constexpr uint16_t VOXL_ESC_DEFAULT_RPM_MIN = 5000; + static constexpr uint16_t VOXL_ESC_DEFAULT_RPM_MAX = 17000; + + static constexpr float VOXL_ESC_MODE_DISABLED_SETPOINT = -0.1f; + static constexpr float VOXL_ESC_MODE_THRESHOLD = 0.0f; - static constexpr float MODAL_IO_MODE_DISABLED_SETPOINT = -0.1f; - static constexpr float MODAL_IO_MODE_THRESHOLD = 0.0f; + static constexpr uint32_t VOXL_ESC_MODE = 0; + static constexpr uint32_t VOXL_ESC_MODE_TURTLE_AUX1 = 1; + static constexpr uint32_t VOXL_ESC_MODE_TURTLE_AUX2 = 2; - static constexpr uint32_t MODAL_IO_MODE = 0; - static constexpr uint32_t MODAL_IO_MODE_TURTLE_AUX1 = 1; - static constexpr uint32_t MODAL_IO_MODE_TURTLE_AUX2 = 2; - static constexpr uint32_t MODAL_IO_MODE_UART_BRIDGE = 3; + static constexpr uint16_t VOXL_ESC_EXT_RPM = 39; + static constexpr uint16_t VOXL_ESC_RPM_MAX = INT16_MAX - + 1; // 32K, Limit max standard range RPM to prevent overflow (rpm packet packing function accepts int32_t) + static constexpr uint16_t VOXL_ESC_RPM_MAX_EXT = UINT16_MAX - + 5; // 65K, Limit max extended range RPM to prevent overflow (rpm packet packing function accepts int32_t) - //static constexpr uint16_t max_pwm(uint16_t pwm) { return math::min(pwm, MODAL_IO_PWM_MAX); } - //static constexpr uint16_t max_rpm(uint16_t rpm) { return math::min(rpm, MODAL_IO_RPM_MAX); } + //static constexpr uint16_t max_pwm(uint16_t pwm) { return math::min(pwm, VOXL_ESC_PWM_MAX); } + //static constexpr uint16_t max_rpm(uint16_t rpm) { return math::min(rpm, VOXL_ESC_RPM_MAX); } - ModalIoSerial *_uart_port; - ModalIoSerial *_uart_port_bridge; + VoxlEscSerial *_uart_port; typedef struct { - int32_t config{MODAL_IO_UART_CONFIG}; - int32_t mode{MODAL_IO_MODE}; + int32_t config{VOXL_ESC_UART_CONFIG}; + int32_t mode{VOXL_ESC_MODE}; int32_t turtle_motor_expo{35}; int32_t turtle_motor_deadband{20}; int32_t turtle_motor_percent{90}; float turtle_stick_minf{0.15f}; float turtle_cosphi{0.99f}; - int32_t baud_rate{MODAL_IO_DEFAULT_BAUD}; - int32_t rpm_min{MODAL_IO_DEFAULT_RPM_MIN}; - int32_t rpm_max{MODAL_IO_DEFAULT_RPM_MAX}; - int32_t function_map[MODAL_IO_OUTPUT_CHANNELS] {0, 0, 0, 0}; - int32_t motor_map[MODAL_IO_OUTPUT_CHANNELS] {1, 2, 3, 4}; - int32_t direction_map[MODAL_IO_OUTPUT_CHANNELS] {1, 1, 1, 1}; + int32_t baud_rate{VOXL_ESC_DEFAULT_BAUD}; + int32_t rpm_min{VOXL_ESC_DEFAULT_RPM_MIN}; + int32_t rpm_max{VOXL_ESC_DEFAULT_RPM_MAX}; + int32_t function_map[VOXL_ESC_OUTPUT_CHANNELS] {0, 0, 0, 0}; + int32_t motor_map[VOXL_ESC_OUTPUT_CHANNELS] {1, 2, 3, 4}; + int32_t direction_map[VOXL_ESC_OUTPUT_CHANNELS] {1, 1, 1, 1}; int32_t verbose_logging{0}; - } modal_io_params_t; + int32_t publish_battery_status{0}; + } voxl_esc_params_t; struct EscChan { - int16_t rate_req; + int32_t rate_req; uint8_t state; - uint16_t rate_meas; + uint32_t rate_meas; uint8_t power_applied; uint8_t led; uint8_t cmd_counter; @@ -167,14 +175,14 @@ class ModalIo : public ModuleBase, public OutputModuleInterface typedef struct { led_control_s control{}; vehicle_control_mode_s mode{}; - uint8_t led_mask;// TODO led_mask[MODAL_IO_OUTPUT_CHANNELS]; + uint8_t led_mask;// TODO led_mask[VOXL_ESC_OUTPUT_CHANNELS]; bool breath_en; uint8_t breath_counter; bool test; } led_rsc_t; - ch_assign_t _output_map[MODAL_IO_OUTPUT_CHANNELS] {{1, 1}, {2, 1}, {3, 1}, {4, 1}}; - MixingOutput _mixing_output{"MODAL_IO", MODAL_IO_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; + ch_assign_t _output_map[VOXL_ESC_OUTPUT_CHANNELS] {{1, 1}, {2, 1}, {3, 1}, {4, 1}}; + MixingOutput _mixing_output; perf_counter_t _cycle_perf; perf_counter_t _output_update_perf; @@ -188,13 +196,19 @@ class ModalIo : public ModuleBase, public OutputModuleInterface uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; uORB::Subscription _actuator_test_sub{ORB_ID(actuator_test)}; uORB::Subscription _led_update_sub{ORB_ID(led_control)}; + uORB::Subscription _voxl2_io_data_sub{ORB_ID(voxl2_io_data)}; uORB::Publication _outputs_debug_pub{ORB_ID(actuator_outputs_debug)}; uORB::Publication _esc_status_pub{ORB_ID(esc_status)}; - modal_io_params_t _parameters; + bool _extended_rpm{false}; + bool _need_version_info{true}; + QC_ESC_VERSION_INFO _version_info[4]; + bool check_versions_updated(); + + voxl_esc_params_t _parameters; int update_params(); - int load_params(modal_io_params_t *params, ch_assign_t *map); + int load_params(voxl_esc_params_t *params, ch_assign_t *map); bool _turtle_mode_en{false}; int32_t _rpm_turtle_min{0}; @@ -205,11 +219,10 @@ class ModalIo : public ModuleBase, public OutputModuleInterface Command _current_cmd; px4::atomic _pending_cmd{nullptr}; - EscChan _esc_chans[MODAL_IO_OUTPUT_CHANNELS]; + EscChan _esc_chans[VOXL_ESC_OUTPUT_CHANNELS]; Command _esc_cmd; esc_status_s _esc_status; EscPacket _fb_packet; - EscPacket _uart_bridge_packet; led_rsc_t _led_rsc; int _fb_idx; @@ -219,6 +232,10 @@ class ModalIo : public ModuleBase, public OutputModuleInterface static const uint8_t READ_BUF_SIZE = 128; uint8_t _read_buf[READ_BUF_SIZE]; + Battery _battery; + static constexpr unsigned _battery_report_interval{100_ms}; + hrt_abstime _last_battery_report_time; + void update_leds(vehicle_control_mode_s mode, led_control_s control); int read_response(Command *out_cmd); diff --git a/src/drivers/actuators/modal_io/modal_io_params.c b/src/drivers/actuators/voxl_esc/voxl_esc_params.c similarity index 73% rename from src/drivers/actuators/modal_io/modal_io_params.c rename to src/drivers/actuators/voxl_esc/voxl_esc_params.c index 3bb848985595..8c1d03c6e7c1 100644 --- a/src/drivers/actuators/modal_io/modal_io_params.c +++ b/src/drivers/actuators/voxl_esc/voxl_esc_params.c @@ -38,23 +38,23 @@ * * @reboot_required true * - * @group MODAL IO + * @group VOXL ESC * @value 0 - Disabled * @value 1 - VOXL ESC * @min 0 * @max 1 */ -PARAM_DEFINE_INT32(MODAL_IO_CONFIG, 0); +PARAM_DEFINE_INT32(VOXL_ESC_CONFIG, 0); /** * UART ESC baud rate * * Default rate is 250Kbps, which is used in off-the-shelf MoadalAI ESC products. * - * @group MODAL IO + * @group VOXL ESC * @unit bit/s */ -PARAM_DEFINE_INT32(MODAL_IO_BAUD, 250000); +PARAM_DEFINE_INT32(VOXL_ESC_BAUD, 250000); /** * Motor mappings for ModalAI ESC @@ -68,33 +68,33 @@ PARAM_DEFINE_INT32(MODAL_IO_BAUD, 250000); // The following are auto generated params from control allocator pattern, put here for reference // Default ESC1 to motor2 -//PARAM_DEFINE_INT32(MODAL_IO_FUNC1, 102); +//PARAM_DEFINE_INT32(VOXL_ESC_FUNC1, 102); -//PARAM_DEFINE_INT32(MODAL_IO_FUNC2, 103); +//PARAM_DEFINE_INT32(VOXL_ESC_FUNC2, 103); -//PARAM_DEFINE_INT32(MODAL_IO_FUNC3, 101); +//PARAM_DEFINE_INT32(VOXL_ESC_FUNC3, 101); -//PARAM_DEFINE_INT32(MODAL_IO_FUNC4, 104); +//PARAM_DEFINE_INT32(VOXL_ESC_FUNC4, 104); /** * UART ESC RPM Min * * Minimum RPM for ESC * - * @group MODAL IO + * @group VOXL ESC * @unit rpm */ -PARAM_DEFINE_INT32(MODAL_IO_RPM_MIN, 5500); +PARAM_DEFINE_INT32(VOXL_ESC_RPM_MIN, 5500); /** * UART ESC RPM Max * * Maximum RPM for ESC * - * @group MODAL IO + * @group VOXL ESC * @unit rpm */ -PARAM_DEFINE_INT32(MODAL_IO_RPM_MAX, 15000); +PARAM_DEFINE_INT32(VOXL_ESC_RPM_MAX, 15000); /** * UART ESC Mode @@ -103,7 +103,7 @@ PARAM_DEFINE_INT32(MODAL_IO_RPM_MAX, 15000); * * @reboot_required true * - * @group MODAL IO + * @group VOXL ESC * @value 0 - None * @value 1 - Turtle Mode enabled via AUX1 * @value 2 - Turtle Mode enabled via AUX2 @@ -111,108 +111,124 @@ PARAM_DEFINE_INT32(MODAL_IO_RPM_MAX, 15000); * @min 0 * @max 2 */ -PARAM_DEFINE_INT32(MODAL_IO_MODE, 0); +PARAM_DEFINE_INT32(VOXL_ESC_MODE, 0); /** * UART ESC ID 1 Spin Direction Flag * - * @group MODAL IO + * @group VOXL ESC * @value 0 - Default * @value 1 - Reverse */ -PARAM_DEFINE_INT32(MODAL_IO_SDIR1, 0); +PARAM_DEFINE_INT32(VOXL_ESC_SDIR1, 0); /** * UART ESC ID 2 Spin Direction Flag * - * @group MODAL IO + * @group VOXL ESC * @value 0 - Default * @value 1 - Reverse */ -PARAM_DEFINE_INT32(MODAL_IO_SDIR2, 0); +PARAM_DEFINE_INT32(VOXL_ESC_SDIR2, 0); /** * UART ESC ID 3 Spin Direction Flag * - * @group MODAL IO + * @group VOXL ESC * @value 0 - Default * @value 1 - Reverse */ -PARAM_DEFINE_INT32(MODAL_IO_SDIR3, 0); +PARAM_DEFINE_INT32(VOXL_ESC_SDIR3, 0); /** * UART ESC ID 4 Spin Direction Flag * - * @group MODAL IO + * @group VOXL ESC * @value 0 - Default * @value 1 - Reverse */ -PARAM_DEFINE_INT32(MODAL_IO_SDIR4, 0); +PARAM_DEFINE_INT32(VOXL_ESC_SDIR4, 0); /** * UART ESC Turtle Mode Crash Flip Motor Percent * - * @group MODAL IO + * @group VOXL ESC * @min 1 * @max 100 * @decimal 10 * @increment 1 */ -PARAM_DEFINE_INT32(MODAL_IO_T_PERC, 90); +PARAM_DEFINE_INT32(VOXL_ESC_T_PERC, 90); /** * UART ESC Turtle Mode Crash Flip Motor Deadband * - * @group MODAL IO + * @group VOXL ESC * @min 0 * @max 100 * @decimal 10 * @increment 1 */ -PARAM_DEFINE_INT32(MODAL_IO_T_DEAD, 20); +PARAM_DEFINE_INT32(VOXL_ESC_T_DEAD, 20); /** * UART ESC Turtle Mode Crash Flip Motor STICK_MINF * - * @group MODAL IO + * @group VOXL ESC * @min 0.0 * @max 100.0 * @decimal 10 * @increment 1.0 */ -PARAM_DEFINE_FLOAT(MODAL_IO_T_MINF, 0.15); +PARAM_DEFINE_FLOAT(VOXL_ESC_T_MINF, 0.15); /** * UART ESC Turtle Mode Crash Flip Motor expo * - * @group MODAL IO + * @group VOXL ESC * @min 0 * @max 100 * @decimal 10 * @increment 1 */ -PARAM_DEFINE_INT32(MODAL_IO_T_EXPO, 35); +PARAM_DEFINE_INT32(VOXL_ESC_T_EXPO, 35); /** * UART ESC Turtle Mode Cosphi * - * @group MODAL IO + * @group VOXL ESC * @min 0.000 * @max 1.000 * @decimal 10 * @increment 0.001 */ -PARAM_DEFINE_FLOAT(MODAL_IO_T_COSP, 0.990); +PARAM_DEFINE_FLOAT(VOXL_ESC_T_COSP, 0.990); /** * UART ESC verbose logging * * @reboot_required true * - * @group MODAL IO + * @group VOXL ESC * @value 0 - Disabled * @value 1 - Enabled * @min 0 * @max 1 */ -PARAM_DEFINE_INT32(MODAL_IO_VLOG, 0); +PARAM_DEFINE_INT32(VOXL_ESC_VLOG, 0); + + +/** + * UART ESC Enable publishing of battery status + * + * Only applicable to ESCs that report total battery voltage and current + * + * @reboot_required true + * + * @group VOXL ESC + * @value 0 - Disabled + * @value 1 - Enabled + * @min 0 + * @max 1 + */ +PARAM_DEFINE_INT32(VOXL_ESC_PUB_BST, 1); \ No newline at end of file diff --git a/src/drivers/actuators/modal_io/modal_io_serial.cpp b/src/drivers/actuators/voxl_esc/voxl_esc_serial.cpp similarity index 93% rename from src/drivers/actuators/modal_io/modal_io_serial.cpp rename to src/drivers/actuators/voxl_esc/voxl_esc_serial.cpp index f4b807673048..1064dc4365c3 100644 --- a/src/drivers/actuators/modal_io/modal_io_serial.cpp +++ b/src/drivers/actuators/voxl_esc/voxl_esc_serial.cpp @@ -32,20 +32,20 @@ ****************************************************************************/ #include "string.h" -#include "modal_io_serial.hpp" +#include "voxl_esc_serial.hpp" -ModalIoSerial::ModalIoSerial() +VoxlEscSerial::VoxlEscSerial() { } -ModalIoSerial::~ModalIoSerial() +VoxlEscSerial::~VoxlEscSerial() { if (_uart_fd >= 0) { uart_close(); } } -int ModalIoSerial::uart_open(const char *dev, speed_t speed) +int VoxlEscSerial::uart_open(const char *dev, speed_t speed) { if (_uart_fd >= 0) { PX4_ERR("Port in use: %s (%i)", dev, errno); @@ -110,7 +110,7 @@ int ModalIoSerial::uart_open(const char *dev, speed_t speed) return 0; } -int ModalIoSerial::uart_set_baud(speed_t speed) +int VoxlEscSerial::uart_set_baud(speed_t speed) { #ifndef __PX4_QURT @@ -134,7 +134,7 @@ int ModalIoSerial::uart_set_baud(speed_t speed) return -1; } -int ModalIoSerial::uart_close() +int VoxlEscSerial::uart_close() { #ifndef __PX4_QURT @@ -158,7 +158,7 @@ int ModalIoSerial::uart_close() return 0; } -int ModalIoSerial::uart_write(FAR void *buf, size_t len) +int VoxlEscSerial::uart_write(FAR void *buf, size_t len) { if (_uart_fd < 0 || buf == NULL) { PX4_ERR("invalid state for writing or buffer"); @@ -172,7 +172,7 @@ int ModalIoSerial::uart_write(FAR void *buf, size_t len) #endif } -int ModalIoSerial::uart_read(FAR void *buf, size_t len) +int VoxlEscSerial::uart_read(FAR void *buf, size_t len) { if (_uart_fd < 0 || buf == NULL) { PX4_ERR("invalid state for reading or buffer"); diff --git a/src/drivers/actuators/modal_io/modal_io_serial.hpp b/src/drivers/actuators/voxl_esc/voxl_esc_serial.hpp similarity index 97% rename from src/drivers/actuators/modal_io/modal_io_serial.hpp rename to src/drivers/actuators/voxl_esc/voxl_esc_serial.hpp index c91f121ca39d..99e918886f62 100644 --- a/src/drivers/actuators/modal_io/modal_io_serial.hpp +++ b/src/drivers/actuators/voxl_esc/voxl_esc_serial.hpp @@ -43,11 +43,11 @@ #define FAR #endif -class ModalIoSerial +class VoxlEscSerial { public: - ModalIoSerial(); - virtual ~ModalIoSerial(); + VoxlEscSerial(); + virtual ~VoxlEscSerial(); int uart_open(const char *dev, speed_t speed); int uart_set_baud(speed_t speed); diff --git a/src/drivers/barometer/bmp388/bmp388.cpp b/src/drivers/barometer/bmp388/bmp388.cpp index 8178bc5785dd..50c40e0686f8 100644 --- a/src/drivers/barometer/bmp388/bmp388.cpp +++ b/src/drivers/barometer/bmp388/bmp388.cpp @@ -77,6 +77,7 @@ BMP388::init() if (_chip_id == BMP390_CHIP_ID) { _interface->set_device_type(DRV_BARO_DEVTYPE_BMP390); + this->_item_name = "bmp390"; } _chip_rev_id = _interface->get_reg(BMP3_REV_ID_ADDR); diff --git a/src/drivers/barometer/invensense/icp201xx/ICP201XX.cpp b/src/drivers/barometer/invensense/icp201xx/ICP201XX.cpp index 6db6bb76d0a3..4cc21739323a 100755 --- a/src/drivers/barometer/invensense/icp201xx/ICP201XX.cpp +++ b/src/drivers/barometer/invensense/icp201xx/ICP201XX.cpp @@ -144,7 +144,8 @@ ICP201XX::RunImpl() if (version == 0xB2) { /* B2 version Asic is detected. Boot up sequence is not required for B2 Asic, so returning */ _state = STATE::CONFIG; - ScheduleDelayed(10_ms); + ScheduleDelayed(30_ms); + break; } /* Read boot up status and avoid re running boot up sequence if it is already done */ @@ -250,7 +251,7 @@ ICP201XX::RunImpl() case STATE::CONFIG: { if (configure()) { _state = STATE::WAIT_READ; - ScheduleDelayed(30_ms); + ScheduleDelayed(50_ms); } else { if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index fa3b14edb824..913bbbe7b82a 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -392,6 +392,12 @@ int BATT_SMBUS::get_startup_info() uint16_t state_of_health; ret |= _interface->read_word(BATT_SMBUS_STATE_OF_HEALTH, state_of_health); + /* ManufacturerAccess dummy command to init the ManufacturerBlockAccess routine + in the BQ40Zx0 and avoid timeout during LifetimeDataFlush. + test Sleep > 20 ms to give time to init the ManufacturerBlockAccess routine*/ + ret |= _interface->write_word(BATT_SMBUS_MANUFACTURER_ACCESS, BATT_SMBUS_DEVICE_TYPE); + px4_usleep(30_ms); + if (!ret) { _serial_number = serial_num; _batt_startup_capacity = (uint16_t)((float)remaining_cap * _c_mult); diff --git a/src/drivers/batt_smbus/batt_smbus.h b/src/drivers/batt_smbus/batt_smbus.h index 1915865c80fb..b2b1e0b702b9 100644 --- a/src/drivers/batt_smbus/batt_smbus.h +++ b/src/drivers/batt_smbus/batt_smbus.h @@ -107,6 +107,7 @@ using namespace time_literals; #define BATT_SMBUS_SECURITY_KEYS 0x0035 +#define BATT_SMBUS_DEVICE_TYPE 0x0001 #define BATT_SMBUS_LIFETIME_FLUSH 0x002E #define BATT_SMBUS_LIFETIME_BLOCK_ONE 0x0060 #define BATT_SMBUS_ENABLED_PROTECTIONS_A_ADDRESS 0x4938 diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp index c03e63aca45a..4f49e5692baf 100644 --- a/src/drivers/camera_trigger/camera_trigger.cpp +++ b/src/drivers/camera_trigger/camera_trigger.cpp @@ -314,7 +314,7 @@ CameraTrigger::CameraTrigger() : // Advertise critical publishers here, because we cannot advertise in interrupt context camera_trigger_s trigger{}; - _trigger_pub = orb_advertise_queue(ORB_ID(camera_trigger), &trigger, camera_trigger_s::ORB_QUEUE_LENGTH); + _trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trigger); } CameraTrigger::~CameraTrigger() 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 44cdc921683d..3e30649ae104 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 @@ -160,6 +160,11 @@ status_t Argus_InitMode(argus_hnd_t *hnd, s2pi_slave_t spi_slave, argus_mode_t m * Also refer to #Argus_ReinitMode, which uses a specified measurement * mode instead of the currently active measurement mode. * + * @note If a full re-initialization is not desired, refer to the + * #Argus_RestoreDeviceState function that will only re-write the + * register map to the device to restore its state after an power + * cycle. + * * @param hnd The API handle; contains all internal states and data. * * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). @@ -182,6 +187,11 @@ status_t Argus_Reinit(argus_hnd_t *hnd); * Also refer to #Argus_Reinit, which re-uses the currently active * measurement mode instead of an user specified measurement mode. * + * @note If a full re-initialization is not desired, refer to the + * #Argus_RestoreDeviceState function that will only re-write the + * register map to the device to restore its state after an power + * cycle. + * * @param hnd The API handle; contains all internal states and data. * * @param mode The specified measurement mode to be initialized. @@ -274,6 +284,69 @@ argus_hnd_t *Argus_CreateHandle(void); *****************************************************************************/ status_t Argus_DestroyHandle(argus_hnd_t *hnd); +/*!*************************************************************************** + * @brief Restores the device state with a re-write of all register values. + * + * @details The function invalidates and restores the device state by executing + * a re-write of the full register map. + * + * The purpose of this function is to recover from known external + * events like power cycles, for example due to sleep / wake-up + * functionality. This can be implemented by cutting off the external + * power supply of the device (e.g. via a MOSFET switch controlled by + * a GPIB pin). By calling this function, the expected state of the + * API is written to the device without the need to fully re-initialize + * the device. Thus, the API can resume where it has stopped as if + * there has never been a power cycle. + * + * The internal state machines like the dynamic configuration adaption + * (DCA) algorithm will not be reseted. The API/sensor will immediately + * resume at the last state that was optimized for the given + * environmental conditions. + * + * The use case of sleep / wake-up can be implemented as follows: + * + * 1. In case of ongoing measurements, stop the measurements via + * the #Argus_StopMeasurementTimer function (if started by the + * #Argus_StartMeasurementTimer function). + * + * 2. Shut down the device by removing the 5V power supply, e.g. + * via a GPIO pin that switches a MOSFET circuit. + * + * 3. After the desired sleep period, power the device by switching + * the 5V power supply on again. Wait until the power-on-reset + * (POR) is finished (approx. 1 ms) or just repeat step 4 until + * it succeeds. + * + * 4. Call the #Argus_RestoreDeviceState function to trigger the + * restoration of the device state in the API. Note that the + * function will return an error code if it fails. One can repeat + * the execution of that function a few times until it succeeds. + * + * 6. Continue with measurements via #Argus_StartMeasurementTimer + * of #Argus_TriggerMeasurement functions as desired. + * + * @note If a complete re-initialization (= soft-reset) is desired, see + * the #Argus_Reinit functionality. + * + * @note Changing a configuration or calibration parameter will always + * invalidate the device state as well as the state machine of the + * dynamic configuration adaption (DCA) algorithm. In that case, the + * device/API needs a few measurements to adopt to the present + * environmental conditions before the first valid measurement result + * can be obtained. This is almost similar to re-initializing the + * device (see #Argus_Reinit) which would also re-read the EEPROM. + * On the other hand, the #Argus_RestoreDeviceState does not reset + * or re-initialize anything. It just makes sure that the device + * register map (which has changed to its reset values after the + * power cycle) is what the API expects upon the next measurement. + * + * @param hnd The device handle object to be invalidated. + * + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_RestoreDeviceState(argus_hnd_t *hnd); + /*!************************************************************************** * Generic API ****************************************************************************/ @@ -726,7 +799,7 @@ status_t Argus_ExecuteXtalkCalibrationSequence(argus_hnd_t *hnd); * 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. + * #Argus_GetCalibrationGlobalRangeOffsets function. * * @param hnd The API handle; contains all internal states and data. * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). @@ -775,7 +848,7 @@ status_t Argus_ExecuteRelativeRangeOffsetCalibrationSequence(argus_hnd_t *hnd); * 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. + * #Argus_GetCalibrationGlobalRangeOffsets function. * * @param hnd The API handle; contains all internal states and data. * @param targetRange The absolute range between the reference plane and the @@ -1043,28 +1116,40 @@ status_t Argus_GetConfigurationUnambiguousRange(argus_hnd_t *hnd, ****************************************************************************/ /*!*************************************************************************** - * @brief Sets the global range offset value to a specified device. + * @brief Sets the global range offset values to a specified device. * - * @details The global range offset is subtracted from the raw range values. + * @details The global range offsets are subtracted from the raw range values. + * There are two distinct values that are applied in low or high + * power stage setting respectively. * * @param hnd The API handle; contains all internal states and data. - * @param value The new global range offset in meter and Q0.15 format. + * @param offset_low The new global range offset for the low power stage in + * meter and Q0.15 format. + * @param offset_high The new global range offset for the high power stage 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, - q0_15_t value); +status_t Argus_SetCalibrationGlobalRangeOffsets(argus_hnd_t *hnd, + q0_15_t offset_low, + q0_15_t offset_high); /*!*************************************************************************** - * @brief Gets the global range offset value from a specified device. + * @brief Gets the global range offset values from a specified device. * - * @details The global range offset is subtracted from the raw range values. + * @details The global range offsets are subtracted from the raw range values. + * There are two distinct values that are applied in low or high + * power stage setting respectively. * * @param hnd The API handle; contains all internal states and data. - * @param value The current global range offset in meter and Q0.15 format. + * @param offset_low The current range offset for the low power stage in + * meter and Q0.15 format. + * @param offset_high The current global range offset for the high power stage + * 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, - q0_15_t *value); +status_t Argus_GetCalibrationGlobalRangeOffsets(argus_hnd_t *hnd, + q0_15_t *offset_low, + q0_15_t *offset_high); /*!*************************************************************************** * @brief Sets the relative pixel offset table to a specified device. 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 8f6b40bdc5b9..8d9a854b2cc1 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 @@ -210,9 +210,13 @@ typedef enum argus_dca_gain_t { * - [9]: #ARGUS_STATE_LASER_ERROR * - [10]: #ARGUS_STATE_HAS_DATA * - [11]: #ARGUS_STATE_HAS_AUX_DATA - * - [12]: #ARGUS_STATE_DCA_MAX + * - [12]: #ARGUS_STATE_SATURATED_PIXELS * - [13]: DCA Power Stage * - [14-15]: DCA Gain Stages + * - [16]: #ARGUS_STATE_DCA_MIN + * - [17]: #ARGUS_STATE_DCA_MAX + * - [18]: #ARGUS_STATE_DCA_RESET + * - [18-31]: not used * . *****************************************************************************/ typedef enum argus_state_t { @@ -229,36 +233,35 @@ typedef enum argus_state_t { * - 1: Enabled: measurement with detuned frequency. */ ARGUS_STATE_DUAL_FREQ_MODE = 1U << 1U, - /*! 0x0004: Measurement Frequency for Dual Frequency Mode + /*! 0x0004: Measurement Frequency for Dual Frequency Mode \n * (only if #ARGUS_STATE_DUAL_FREQ_MODE flag is set). * - 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 + /*! 0x0008: Debug Mode. \n + * If set, the range value of erroneous pixels * are not cleared or reset. * - 0: Disabled (default). * - 1: Enabled. */ ARGUS_STATE_DEBUG_MODE = 1U << 3U, - /*! 0x0010: Weak Signal Flag. + /*! 0x0010: Weak Signal Flag. \n * 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. + * (absolute) threshold. * - 0: Normal Signal. - * - 1: Weak Signal or Golden Pixel Mode. */ + * - 1: Weak Signal. */ ARGUS_STATE_WEAK_SIGNAL = 1U << 4U, - /*! 0x0020: Background Light Warning Flag. + /*! 0x0020: Background Light Warning Flag. \n * 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. + /*! 0x0040: Background Light Error Flag. \n * 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. @@ -270,7 +273,7 @@ typedef enum argus_state_t { * - 1: PLL locked at start of integration. */ ARGUS_STATE_PLL_LOCKED = 1U << 7U, - /*! 0x0100: Laser Failure Warning Flag. + /*! 0x0100: Laser Failure Warning Flag. \n * 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). @@ -279,7 +282,7 @@ typedef enum argus_state_t { * condition stays, a laser malfunction error is raised. */ ARGUS_STATE_LASER_WARNING = 1U << 8U, - /*! 0x0200: Laser Failure Error Flag. + /*! 0x0200: Laser Failure Error Flag. \n * Set whenever a laser malfunction error is raised and the * system is put into a safe state. * - 0: No Error: Laser is operating properly. @@ -297,13 +300,12 @@ typedef enum argus_state_t { * - 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. */ - ARGUS_STATE_DCA_MAX = 1U << 12U, + /*! 0x0100: Pixel Saturation Flag. \n + * Set whenever any pixel is saturated, i.e. its pixel state is + * #PIXEL_SAT + * - 0: No saturated pixels. + * - 1: Any saturated pixels. */ + ARGUS_STATE_SATURATED_PIXELS = 1U << 12U, /*! 0x2000: DCA is in high Optical Output Power stage. */ ARGUS_STATE_DCA_POWER_HIGH = DCA_POWER_HIGH << ARGUS_STATE_DCA_POWER_SHIFT, @@ -320,6 +322,31 @@ typedef enum argus_state_t { /*! 0xC000: DCA is in high Pixel Input Gain stage. */ ARGUS_STATE_DCA_GAIN_HIGH = DCA_GAIN_HIGH << ARGUS_STATE_DCA_GAIN_SHIFT, + /*! 0x10000: DCA Minimum State Flag. \n + * Set whenever the DCA has reduced all its parameters to their + * minimum values and it can not decrease the integration energy + * any further. + * - 0: DCA has not yet reached its minimum state. + * - 1: DCA has reached its minimum state and can not decrease + * its parameters any further. */ + ARGUS_STATE_DCA_MIN = 1U << 16U, + + /*! 0x20000: DCA Maximum State Flag. \n + * Set whenever the DCA has extended all its parameters to their + * maximum values and it 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 + * its parameters any further. */ + ARGUS_STATE_DCA_MAX = 1U << 17U, + + /*! 0x20000: DCA Reset State Flag. \n + * Set whenever the DCA is resetting all its parameters to their + * minimum values because it has detected too many saturated pixels. + * - 0: DCA is operating in normal mode. + * - 1: DCA is performing a reset. */ + ARGUS_STATE_DCA_RESET = 1U << 18U, + } argus_state_t; /*!*************************************************************************** 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 4ffa55656ba2..22a85b9d3e24 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 @@ -58,6 +58,7 @@ extern "C" { *****************************************************************************/ #include "utility/int_math.h" +#include #include @@ -138,6 +139,13 @@ extern "C" { #define PIXEL_CH2N(c) (((((c) << 1U) ^ 0x1CU) & 0x1CU) | (((c) >> 3U) & 0x02U) | ((c) & 0x01U)) +/*!***************************************************************************** + * @brief Macro to create a pixel mask given by the pixels n-index. + * @param n n-index of the pixel. + * @return The pixel mask with only n-index pixel set. + ******************************************************************************/ +#define PIXELN_MASK(n) (0x01U << (n)) + /*!***************************************************************************** * @brief Macro to determine if a pixel given by the n-index is enabled in a pixel mask. * @param msk 32-bit pixel mask @@ -151,15 +159,22 @@ extern "C" { * @param msk 32-bit pixel mask * @param n n-index of the pixel to enable. ******************************************************************************/ -#define PIXELN_ENABLE(msk, n) ((msk) |= (0x01U << (n))) +#define PIXELN_ENABLE(msk, n) ((msk) |= (PIXELN_MASK(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. ******************************************************************************/ -#define PIXELN_DISABLE(msk, n) ((msk) &= (~(0x01U << (n)))) +#define PIXELN_DISABLE(msk, n) ((msk) &= (~PIXELN_MASK(n))) + +/*!***************************************************************************** + * @brief Macro to create a pixel mask given by the pixels ADC channel number. + * @param c The ADC channel number of the pixel. + * @return The 32-bit pixel mask with only pixel ADC channel set. + ******************************************************************************/ +#define PIXELCH_MASK(c) (0x01U << (PIXEL_CH2N(c))) /*!***************************************************************************** * @brief Macro to determine if an ADC pixel channel is enabled from a pixel mask. @@ -184,6 +199,14 @@ extern "C" { #define PIXELCH_DISABLE(msk, c) (PIXELN_DISABLE(msk, PIXEL_CH2N(c))) +/*!***************************************************************************** + * @brief Macro to create a pixel mask given by the pixel x-y-indices. + * @param x x-index of the pixel. + * @param y y-index of the pixel. + * @return The 32-bit pixel mask with only pixel ADC channel set. + ******************************************************************************/ +#define PIXELXY_MASK(x, y) (0x01U << (PIXEL_XY2N(x, y))) + /*!***************************************************************************** * @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 @@ -337,10 +360,10 @@ static inline uint32_t ShiftSelectedPixels(const uint32_t 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; + for (int8_t x = 0; x < ARGUS_PIXELS_X; ++x) { + for (int8_t y = 0; y < ARGUS_PIXELS_Y; ++y) { + int8_t x_src = (int8_t)(x - dx); + int8_t y_src = (int8_t)(y - dy); if (dy & 0x1) { /* Compensate for hexagonal pixel shape. */ @@ -409,8 +432,8 @@ static inline uint32_t FillPixelMask(uint32_t pixel_mask, 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) { + for (int8_t x = 0; x < ARGUS_PIXELS_X; ++x) { + for (int8_t y = 0; y < ARGUS_PIXELS_Y; ++y) { if (!PIXELXY_ISENABLED(pixel_mask, x, y)) { int32_t distx = (x - center_x) << 1; @@ -423,8 +446,8 @@ static inline uint32_t FillPixelMask(uint32_t pixel_mask, if (dist < min_dist) { min_dist = dist; - min_x = x; - min_y = y; + min_x = (int8_t)x; + min_y = (int8_t)y; } } } @@ -438,6 +461,64 @@ static inline uint32_t FillPixelMask(uint32_t pixel_mask, return pixel_mask; } + +/*!***************************************************************************** + * @brief Fills a pixel mask with the direct neighboring pixels around a pixel. + * + * @details The pixel mask is iteratively filled with the direct neighbors of the + * specified center pixel. + * + * Note that the function is able to handle corner and edge pixels and + * also to handle odd/even lines (which have different layouts) + * + * @param x The selected pixel x-index. + * @param y The selected pixel y-index. + * @return The filled pixel mask with all direct neighbors of the selected pixel. + ******************************************************************************/ +static inline uint32_t GetAdjacentPixelsMask(const uint_fast8_t x, + const uint_fast8_t y) +{ + assert(x < ARGUS_PIXELS_X); + assert(y < ARGUS_PIXELS_Y); + + uint32_t mask = 0u; + + bool isXEdgeLow = (x == 0); + bool isXEdgeHigh = (x == (ARGUS_PIXELS_X - 1)); + bool isYEdgeLow = (y == 0); + bool isYEdgeHigh = (y == (ARGUS_PIXELS_Y - 1)); + + if (y % 2 == 0) { + if (!isYEdgeLow) { PIXELXY_ENABLE(mask, x, y - 1); } + + if ((!isXEdgeHigh) && (!isYEdgeLow)) { PIXELXY_ENABLE(mask, x + 1, y - 1); } + + if (!isXEdgeHigh) { PIXELXY_ENABLE(mask, x + 1, y); } + + if ((!isXEdgeHigh) && (!isYEdgeHigh)) { PIXELXY_ENABLE(mask, x + 1, y + 1); } + + if (!isYEdgeHigh) { PIXELXY_ENABLE(mask, x, y + 1); } + + if (!isXEdgeLow) { PIXELXY_ENABLE(mask, x - 1, y); } + + } else { + if ((!isXEdgeLow) && (!isYEdgeLow)) { PIXELXY_ENABLE(mask, x - 1, y - 1); } + + if (!isYEdgeLow) { PIXELXY_ENABLE(mask, x, y - 1); } + + if (!isXEdgeHigh) { PIXELXY_ENABLE(mask, x + 1, y); } + + if (!isYEdgeHigh) { PIXELXY_ENABLE(mask, x, y + 1); } + + if ((!isXEdgeLow) && (!isYEdgeHigh)) { PIXELXY_ENABLE(mask, x - 1, y + 1); } + + if (!isXEdgeLow) { PIXELXY_ENABLE(mask, x - 1, y); } + } + + return mask; +} + + /*! @} */ #ifdef __cplusplus } // extern "C" diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_msk.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_msk.h deleted file mode 100644 index 258fb3826092..000000000000 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_msk.h +++ /dev/null @@ -1,170 +0,0 @@ -/*************************************************************************//** - * @file - * @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 - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright 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_MSK_H -#define ARGUS_MSK_H - -/*!*************************************************************************** - * @defgroup argusmap ADC Channel Mapping - * @ingroup argusres - * - * @brief Pixel ADC Channel (n) to x-y-Index Mapping - * - * @details The ADC Channels of each pixel or auxiliary channel on the device - * is numbered in a way that is convenient on the chip. The macros - * in this module are defined in order to obtain the x-y-indices of - * each channel and vice versa. - * - * @addtogroup argusmap - * @{ - *****************************************************************************/ - -#include "api/argus_def.h" -#include "utility/int_math.h" - -/*!***************************************************************************** - * @brief Macro to determine the channel number of an specified Pixel. - * @param x The x index of the pixel. - * @param y The y index of the pixel. - * @return The channel number n of the pixel. - ******************************************************************************/ -#define PIXEL_XY2N(x, y) ((((x) ^ 7) << 1) | ((y) & 2) << 3 | ((y) & 1)) - -/*!***************************************************************************** - * @brief Macro to determine the x index of an specified Pixel channel. - * @param n The channel number of the pixel. - * @return The x index number of the pixel. - ******************************************************************************/ -#define PIXEL_N2X(n) ((((n) >> 1U) & 7) ^ 7) - -/*!***************************************************************************** - * @brief Macro to determine the y index of an specified Pixel channel. - * @param n The channel number of the pixel. - * @return The y index number of the pixel. - ******************************************************************************/ -#define PIXEL_N2Y(n) (((n) & 1U) | (((n) >> 3) & 2U)) - -/*!***************************************************************************** - * @brief Macro to determine if a ADC Pixel channel was enabled from a pixel mask. - * @param msk The 32-bit pixel mask - * @param ch The channel number of the pixel. - * @return True if the pixel channel n was enabled, false elsewise. - ******************************************************************************/ -#define PIXELN_ISENABLED(msk, ch) (((msk) >> (ch)) & 0x01U) - -/*!***************************************************************************** - * @brief Macro enables an ADC Pixel channel in a pixel mask. - * @param msk The 32-bit pixel mask - * @param ch The channel number of the pixel. - ******************************************************************************/ -#define PIXELN_ENABLE(msk, ch) ((msk) |= (0x01U << (ch))) - -/*!***************************************************************************** - * @brief Macro disables an ADC Pixel channel in a pixel mask. - * @param msk The 32-bit pixel mask - * @param ch The channel number of the pixel. - ******************************************************************************/ -#define PIXELN_DISABLE(msk, ch) ((msk) &= (~(0x01U << (ch)))) - -/*!***************************************************************************** - * @brief Macro to determine if an ADC Pixel channel was enabled from 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) was enabled, false elsewise. - ******************************************************************************/ -#define PIXELXY_ISENABLED(msk, x, y) (PIXELN_ISENABLED(msk, PIXEL_XY2N(x, y))) - -/*!***************************************************************************** - * @brief Macro enables an ADC Pixel channel in a pixel mask. - * @param msk 32-bit pixel mask - * @param x x index of the pixel. - * @param y y index of the pixel. - ******************************************************************************/ -#define PIXELXY_ENABLE(msk, x, y) (PIXELN_ENABLE(msk, PIXEL_XY2N(x, y))) - -/*!***************************************************************************** - * @brief Macro disables an ADC Pixel channel in a pixel mask. - * @param msk 32-bit pixel mask - * @param x x index of the pixel. - * @param y y index of the pixel. - ******************************************************************************/ -#define PIXELXY_DISABLE(msk, x, y) (PIXELN_DISABLE(msk, PIXEL_XY2N(x, y))) - -/*!***************************************************************************** - * @brief Macro to determine if a ADC channel was enabled from a channel mask. - * @param msk 32-bit channel mask - * @param ch channel number of the ADC channel. - * @return True if the ADC channel n was enabled, false elsewise. - ******************************************************************************/ -#define CHANNELN_ISENABLED(msk, ch) (((msk) >> ((ch) - 32U)) & 0x01U) - -/*!***************************************************************************** - * @brief Macro to determine if a ADC channel was enabled from a channel mask. - * @param msk 32-bit channel mask - * @param ch channel number of the ADC channel. - * @return True if the ADC channel n was enabled, false elsewise. - ******************************************************************************/ -#define CHANNELN_ENABLE(msk, ch) ((msk) |= (0x01U << ((ch) - 32U))) - -/*!***************************************************************************** - * @brief Macro to determine if a ADC channel was enabled from a channel mask. - * @param msk 32-bit channel mask - * @param ch channel number of the ADC channel. - * @return True if the ADC channel n was enabled, false elsewise. - ******************************************************************************/ -#define CHANNELN_DISABLE(msk, ch) ((msk) &= (~(0x01U << ((ch) - 32U)))) - - -/*!***************************************************************************** - * @brief Macro to determine the number of enabled pixel channels 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. - ******************************************************************************/ -#define CHANNEL_COUNT(pxmsk, chmsk) (popcount(pxmsk) + popcount(chmsk)) - -/*! @} */ -#endif /* ARGUS_MSK_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 index 7a41440f3976..3ef649d45b3b 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_offset.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_offset.h @@ -36,6 +36,9 @@ #ifndef ARGUS_OFFSET_H #define ARGUS_OFFSET_H +#ifdef __cplusplus +extern "C" { +#endif /*!*************************************************************************** * @addtogroup argus_cal @@ -48,12 +51,26 @@ * @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]; +typedef union argus_cal_offset_table_t { + struct { + /*! The offset values table for Low Power Stage of all 32 pixels. + * Unit: meter; Format: Q0.15 */ + q0_15_t LowPower[ARGUS_PIXELS_X][ARGUS_PIXELS_Y]; + + /*! The offset values table for High Power Stage of all 32 pixels. + * Unit: meter; Format: Q0.15 */ + q0_15_t HighPower[ARGUS_PIXELS_X][ARGUS_PIXELS_Y]; + }; + + /*! The offset values table for Low/High Power Stages of all 32 pixels. + * Unit: meter; Format: Q0.15 */ + q0_15_t Table[ARGUS_DCA_POWER_STAGE_COUNT][ARGUS_PIXELS_X][ARGUS_PIXELS_Y]; } argus_cal_offset_table_t; /*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif #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 f28576500d11..f412229396a6 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 @@ -55,11 +55,11 @@ extern "C" { * information from the filtered pixels by averaging them in a * specified way. * - * The Pixel Binning Algorithm is a three-stage filter with a - * fallback value: + * Basically, the Pixel Binning Algorithm is a multi-stage filter: * * -# 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 @@ -75,12 +75,28 @@ extern "C" { * 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. + * result if the maximum amplitude is already very low.\n + * Those threshold are implemented using a hysteresis behavior. + * For its configuration, see the following parameters: + * - #argus_cfg_pba_t::RelativeAmplitudeInclusion + * - #argus_cfg_pba_t::RelativeAmplitudeExclusion + * - #argus_cfg_pba_t::AbsoluteAmplitudeInclusion + * - #argus_cfg_pba_t::AbsoluteAmplitudeExclusion + * . + * * -# 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. + * not correct negative distance.\n + * For its configuration, see the following parameters: + * - #PBA_ENABLE_MIN_DIST_SCOPE + * - #argus_cfg_pba_t::AbsoluteDistanceScopeInclusion + * - #argus_cfg_pba_t::AbsoluteDistanceScopeExclusion + * - #argus_cfg_pba_t::RelativeDistanceScopeInclusion + * - #argus_cfg_pba_t::RelativeDistanceScopeExclusion + * . + * * -# 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 @@ -90,11 +106,31 @@ extern "C" { * 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. + * to heed the increasing noise at larger distances.\n + * For its configuration, see the following parameters: + * - #argus_cfg_pba_t::AbsoluteMinimumDistanceThreshold + * . + * * -# 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. + * of the optics at large distances. Thus, it is expected to + * have the best signal at large distances.\n + * For its configuration, see the following parameters: + * - #PBA_ENABLE_GOLDPX_FALLBACK_MODE + * . + * + * -# In order to avoid unwanted effects from "out-of-focus" pixels + * in application that require a smaller focus, the Golden Pixel + * Priority Mode prioritizes a valid signal on the central + * Golden Pixel over other pixels. That is, while the Golden + * Pixel has a reasonable signal strength, it is the only pixel + * considered for the 1D result.\n + * For its configuration, see the following parameters: + * - #PBA_ENABLE_GOLDPX_FALLBACK_MODE + * - #argus_cfg_pba_t::GoldenPixelPriorityAmplitudeInclusion + * - #argus_cfg_pba_t::GoldenPixelPriorityAmplitudeExclusion + * . * . * * After filtering is done, there may be more than a single pixel @@ -113,14 +149,17 @@ extern "C" { * @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. + * - [4]: #PBA_ENABLE_GOLDPX_PRIORITY_MODE: Enables the Golden Pixel + * priority mode feature. + * - [5]: #PBA_ENABLE_GOLDPX_FALLBACK_MODE: Enables the Golden Pixel + * fallback mode feature. + * - [6]: #PBA_ENABLE_MIN_DIST_SCOPE: Enables the minimum distance + * scope feature. * - [7]: reserved * . *****************************************************************************/ @@ -128,8 +167,17 @@ typedef enum argus_pba_flags_t { /*! Enables the pixel binning feature. */ PBA_ENABLE = 1U << 0U, - /*! Enables the Golden Pixel. */ - PBA_ENABLE_GOLDPX = 1U << 5U, + /*! Enables the Golden Pixel Priority Mode. + * If enabled, the Golden Pixel is prioritized over other Pixels as long + * as it has a good signal (determined by # */ + PBA_ENABLE_GOLDPX_PRIORITY_MODE = 1U << 4U, + + /*! Enables the Golden Pixel Fallback Mode. + * If enabled, the Golden Pixel is used as a last fallback pixel to obtain + * a valid signal from. This is recommended for all non-multi pixel + * devices whose TX field-of-view is aligned to target the Golden Pixel in + * factory calibration. */ + PBA_ENABLE_GOLDPX_FALLBACK_MODE = 1U << 5U, /*! Enables the minimum distance scope filter. */ PBA_ENABLE_MIN_DIST_SCOPE = 1U << 6U, @@ -168,65 +216,297 @@ typedef struct { * about the individual evaluation modes. */ argus_pba_averaging_mode_t AveragingMode; - /*! The Relative amplitude threshold value (in %) of the max. amplitude. + /*! The relative amplitude inclusion threshold (in %) of the max. amplitude. * - * Pixels with amplitude below this threshold value are dismissed. + * Pixels, whose amplitudes raise above this inclusion threshold, are + * added to the pixel binning. The amplitude must fall below the + * exclusion (#RelativeAmplitudeExclusion) threshold to be removed from + * the pixel binning again. * * All available values from the 8-bit representation are valid. * The actual percentage value is determined by 100%/256*x. * - * Use 0 to disable the relative amplitude threshold. */ - uq0_8_t RelAmplThreshold; + * Note: in addition to the relative criteria, there is also the absolute + * criteria (#AbsoluteAmplitudeInclusion, #AbsoluteAmplitudeExclusion). + * The pixels are added to the pixel binning if their respective amplitude + * is larger than the absolute AND relative inclusion values. On the other + * hand, they are removed if their amplitude falls below the absolute OR + * relative exclusion threshold. + * + * Must be greater than or equal to the #RelativeAmplitudeExclusion. + * + * Use #RelativeAmplitudeExclusion == #RelativeAmplitudeInclusion to + * disable the hysteresis behavior and use it as a threshold only. + * + * Use 0 (for both, #RelativeAmplitudeExclusion and + * #RelativeAmplitudeInclusion) to disable the relative amplitude + * hysteresis. */ + uq0_8_t RelativeAmplitudeInclusion; - /*! The relative minimum distance scope value in %. + /*! The relative amplitude exclusion threshold (in %) of the max. amplitude. * - * 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, whose amplitudes fall below this exclusion threshold, are + * removed from the pixel binning. The amplitude must raise above the + * inclusion (#RelativeAmplitudeInclusion) threshold to be added back + * to be pixel binning again. * * All available values from the 8-bit representation are valid. * The actual percentage value is determined by 100%/256*x. * - * 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)! */ - uq0_8_t RelMinDistanceScope; + * Note: in addition to the relative criteria, there is also the absolute + * criteria (#AbsoluteAmplitudeInclusion, #AbsoluteAmplitudeExclusion). + * The pixels are added to the pixel binning if their respective amplitude + * is larger than the absolute AND relative inclusion values. On the other + * hand, they are removed if their amplitude falls below the absolute OR + * relative exclusion threshold. + * + * Must be less than or equal to #RelativeAmplitudeInclusion. + * + * Use #RelativeAmplitudeExclusion == #RelativeAmplitudeInclusion to + * disable the hysteresis behavior and use it as a threshold only. + * + * Use 0 (for both, #RelativeAmplitudeExclusion and + * #RelativeAmplitudeInclusion) to disable the relative amplitude + * hysteresis. */ + uq0_8_t RelativeAmplitudeExclusion; + + /*! The absolute amplitude inclusion threshold in LSB. + * + * Pixels, whose amplitudes raise above this inclusion threshold, are + * added to the pixel binning. The amplitude must fall below the + * exclusion (#RelativeAmplitudeExclusion) threshold to be removed from + * the pixel binning again. + * + * The absolute amplitude hysteresis is only valid if the Golden Pixel + * mode is enabled. Otherwise, the thresholds are set to 0 LSB internally + * which disables the absolute criteria. + * + * All available values from the 16-bit representation are valid. + * The actual LSB value is determined by x/16. + * + * Note: in addition to the absolute criteria, there is also the relative + * criteria (#RelativeAmplitudeInclusion, #RelativeAmplitudeExclusion). + * The pixels are added to the pixel binning if their respective amplitude + * is larger than the absolute AND relative inclusion values. On the other + * hand, they are removed if their amplitude falls below the absolute OR + * relative exclusion threshold. + * + * Must be greater than or equal to #AbsoluteAmplitudeExclusion. + * + * Use #AbsoluteAmplitudeExclusion == #AbsoluteAmplitudeInclusion to + * disable the hysteresis behavior and use it as a threshold only. + * + * Use 0 (for both, #AbsoluteAmplitudeExclusion and + * #AbsoluteAmplitudeInclusion) to disable the absolute amplitude + * hysteresis. */ + uq12_4_t AbsoluteAmplitudeInclusion; + + /*! The absolute amplitude exclusion threshold in LSB. + * + * Pixels, whose amplitudes fall below this exclusion threshold, are + * removed from the pixel binning. The amplitude must raise above the + * inclusion (#RelativeAmplitudeInclusion) threshold to be added back + * to be pixel binning again. + * + * The absolute amplitude hysteresis is only valid if the Golden Pixel + * mode is enabled. Otherwise, the thresholds are set to 0 LSB internally + * which disables the absolute criteria. + * + * All available values from the 16-bit representation are valid. + * The actual LSB value is determined by x/16. + * + * Note: in addition to the absolute criteria, there is also the relative + * criteria (#RelativeAmplitudeInclusion, #RelativeAmplitudeExclusion). + * The pixels are added to the pixel binning if their respective amplitude + * is larger than the absolute AND relative inclusion values. On the other + * hand, they are removed if their amplitude falls below the absolute OR + * relative exclusion threshold. + * + * Must be less than or equal to #AbsoluteAmplitudeInclusion. + * + * Use #AbsoluteAmplitudeExclusion == #AbsoluteAmplitudeInclusion to + * disable the hysteresis behavior and use it as a threshold only. + * + * Use 0 (for both, #AbsoluteAmplitudeExclusion and + * #AbsoluteAmplitudeInclusion) to disable the absolute amplitude + * hysteresis. */ + uq12_4_t AbsoluteAmplitudeExclusion; + + /*! The Golden Pixel Priority Mode inclusion threshold in LSB. + * + * The Golden Pixel Priority Mode prioritizes a valid signal on the + * Golden Pixel over other pixel to avoid unwanted effects from + * "out-of-focus" pixels in application that require a smaller focus. + * + * If the Golden Pixel priority mode is enabled (see + * #PBA_ENABLE_GOLDPX_PRIORITY_MODE) and the Golden Pixel has a valid signal + * with amplitude higher than this inclusion threshold, its priority state + * is enabled and the binning exits early by dismissing all other pixels + * regardless of their respective amplitude or state. The Golden Pixel + * priority state is disabled if the Golden Pixel amplitude falls below + * the exclusion threshold (#GoldenPixelPriorityAmplitudeExclusion) or its + * state becomes invalid (e.g. #PIXEL_SAT). + * + * All available values from the 16-bit representation are valid. + * The actual LSB value is determined by x/16. + * + * Use 0 to disable the Golden Pixel priority mode hysteresis. */ + uq12_4_t GoldenPixelPriorityAmplitudeInclusion; - /*! The absolute amplitude threshold value in LSB. + /*! The Golden Pixel Priority Mode exclusion threshold in LSB. * - * Pixels with amplitude below this threshold value are dismissed. + * The Golden Pixel Priority Mode prioritizes a valid signal on the + * Golden Pixel over other pixel to avoid unwanted effects from + * "out-of-focus" pixels in application that require a smaller focus. * - * The absolute amplitude threshold is only valid if the Golden Pixel - * mode is enabled. Otherwise, the threshold is set to 0 LSB internally. + * If the Golden Pixel priority mode is enabled (see + * #PBA_ENABLE_GOLDPX_PRIORITY_MODE) and the Golden Pixel has a valid + * signal with amplitude higher than the exclusion threshold + * (#GoldenPixelPriorityAmplitudeInclusion), its priority state is enabled + * and the binning exits early by dismissing all other pixels regardless + * of their respective amplitude or state. The Golden Pixel priority state + * is disabled if the Golden Pixel amplitude falls below this exclusion + * threshold or its state becomes invalid (e.g. #PIXEL_SAT). * * All available values from the 16-bit representation are valid. * The actual LSB value is determined by x/16. * - * Use 0 to disable the absolute amplitude threshold. */ - uq12_4_t AbsAmplThreshold; + * Use 0 to disable the Golden Pixel priority mode hysteresis. */ + uq12_4_t GoldenPixelPriorityAmplitudeExclusion; + + /*! The relative minimum distance scope inclusion threshold (in %). + * + * Pixels, whose range is smaller than the minimum distance inclusion + * threshold (x_min + dx_incl) are added to the pixel binning. The + * range must raise above the exclusion + * (#RelativeDistanceScopeExclusion) threshold to be removed + * from the pixel binning again. The relative value is determined + * by multiplying the percentage with the minimum distance. + * + * The distance scope determines an interval within that pixels + * are considered valid, originating at the minimum distance (x_min). + * The width of the interval is specified by the relative and absolute + * minimum distance scope thresholds. The actual values it the + * maximum of both, the relative and absolute inclusion values + * (#AbsoluteDistanceScopeInclusion). + * + * All available values from the 8-bit representation are valid. + * The actual percentage value is determined by 100%/256*x. + * + * Must be smaller than or equal to the #RelativeDistanceScopeExclusion. + * + * Use #RelativeDistanceScopeExclusion == #RelativeDistanceScopeInclusion to + * disable the hysteresis behavior and use it as a threshold only. */ + uq0_8_t RelativeDistanceScopeInclusion; + + /*! The relative distance scope exclusion threshold (in %). + * + * Pixels, whose range is larger than the minimum distance exclusion + * threshold (x_min + dx_excl) are removed from the pixel binning. The + * range must fall below the inclusion + * (#RelativeDistanceScopeInclusion) threshold to be added + * to the pixel binning again. The relative value is determined + * by multiplying the percentage with the minimum distance. + * + * The distance scope determines an interval within that pixels + * are considered valid, originating at the minimum distance (x_min). + * The width of the interval is specified by the relative and absolute + * minimum distance scope thresholds. The actual values it the + * maximum of both, the relative and absolute exclusion values + * (#AbsoluteDistanceScopeExclusion). + * + * All available values from the 8-bit representation are valid. + * The actual percentage value is determined by 100%/256*x. + * + * Must be larger than or equal to the #RelativeDistanceScopeInclusion. + * + * Use #RelativeDistanceScopeExclusion == #RelativeDistanceScopeInclusion to + * disable the hysteresis behavior and use it as a threshold only. */ + uq0_8_t RelativeDistanceScopeExclusion; - /*! The absolute minimum distance scope value in m. + /*! The absolute minimum distance scope inclusion threshold (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, whose range is smaller than the minimum distance inclusion + * threshold (x_min + dx_incl) are added to the pixel binning. The + * range must raise above the exclusion + * (#AbsoluteDistanceScopeExclusion) threshold to be added + * to the pixel binning again. + * + * The distance scope determines an interval within that pixels + * are considered valid, originating at the minimum distance (x_min). + * The width of the interval is specified by the relative and absolute + * minimum distance scope thresholds. The actual values it the + * maximum of both, the relative and absolute exclusion values + * (#RelativeDistanceScopeInclusion). + * + * All available values from the 16-bit representation are valid. + * The actual LSB value is determined by x/2^15. + * + * Must be smaller than or equal to the #AbsoluteDistanceScopeExclusion. + * + * Use #AbsoluteDistanceScopeExclusion == #AbsoluteDistanceScopeInclusion to + * disable the hysteresis behavior and use it as a threshold only. */ + uq1_15_t AbsoluteDistanceScopeInclusion; + + /*! The absolute minimum distance scope exclusion threshold (in m). + * + * Pixels, whose range is larger than the minimum distance exclusion + * threshold (x_min + dx_excl) are removed from the pixel binning. The + * range must fall below the inclusion + * (#AbsoluteDistanceScopeInclusion) threshold to be added + * to the pixel binning again. + * + * The distance scope determines an interval within that pixels + * are considered valid, originating at the minimum distance (x_min). + * The width of the interval is specified by the relative and absolute + * minimum distance scope thresholds. The actual values it the + * maximum of both, the relative and absolute exclusion values + * (#RelativeDistanceScopeExclusion). * * All available values from the 16-bit representation are valid. * The actual LSB value is determined by x/2^15. * - * 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)! */ - uq1_15_t AbsMinDistanceScope; + * Must be larger than or equal to the #AbsoluteDistanceScopeInclusion. + * + * Use #AbsoluteDistanceScopeExclusion == #AbsoluteDistanceScopeInclusion to + * disable the hysteresis behavior and use it as a threshold only. */ + uq1_15_t AbsoluteDistanceScopeExclusion; + + /*! The Golden Pixel Saturation Filter Pixel Threshold. + * + * The Golden Pixel Saturation Filter will evaluate the status of the + * Golden Pixel to #PIXEL_INVALID if a certain number of active pixels, + * i.e. pixels that are not removed by the static pre-filter mask + * (#PrefilterMask), are saturated (#PIXEL_SAT). + * + * The purpose of this filter is to avoid erroneous situations with highly + * reflective targets (e.g. retro-reflectors) that can invalidate the + * Golden Pixel such that it would not show the correct saturation state. + * In order to avoid using the Golden Pixel in that scenario, this filter + * mechanism can be used to remove the Golden Pixel if a specified number + * of other pixels show saturation state. + * + * Use 0 to disable the Golden Pixel Saturation Filter. */ + uint8_t GoldenPixelSaturationFilterPixelThreshold; + + /*! The Golden Pixel out-of-sync age limit for the GPPM. + * + * The Golden Pixel out-of-sync age is the number of consecutive frames + * where the Golden Pixel is out-of-sync. This parameters is the threshold + * to distinguish between temporary and permanent out-of-sync states. + * + * Temporary out-of-sync states happen when the target rapidly changes. In + * this case, the Golden Pixel Priority Mode (GPPM) is not exited. Only if + * the out-of-sync age exceeds the specified threshold, the Golden Pixel is + * considered erroneous and the GPPM is exited. + * + * Use 0 to disable the Golden Pixel out-of-sync aging (= infinity). */ + uint8_t GoldenPixelOutOfSyncAgeThreshold; /*! The absolute minimum distance threshold value in m. * * Pixels with distance below this threshold value are dismissed. */ - q9_22_t AbsMinDistanceThreshold; + q9_22_t AbsoluteMinimumDistanceThreshold; /*! The pre-filter pixel mask determines the pixel channels that are * statically excluded from the pixel binning (i.e. 1D distance) result. 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 a739cea7f367..3d4ef3d50ce4 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 @@ -55,6 +55,9 @@ extern "C" { * Also used as a special value to determine no object detected or infinity range. */ #define ARGUS_RANGE_MAX (Q9_22_MAX) +/*! Minimum range value in Q9.22 format. */ +#define ARGUS_RANGE_MIN (Q9_22_MIN) + /*!*************************************************************************** * @brief Status flags for the evaluated pixel structure. * 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 77cd85641363..8f3fb09688f4 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 @@ -227,12 +227,19 @@ enum Status { /*! -114: AFBR-S50 Error: Register data integrity is lost (e.g. due to unexpected * power-on-reset cycle or invalid write cycle of SPI. System tries to - * reset the values. */ + * reset the values. + * + * @note If this error occurs after intentionally cycling the power supply + * of the device, use the #Argus_RestoreDeviceState API function to properly + * recover the current API state into the device to avoid that issue. */ ERROR_ARGUS_DATA_INTEGRITY_LOST = -114, /*! -115: AFBR-S50 Error: The range offsets calibration failed! */ ERROR_ARGUS_RANGE_OFFSET_CALIBRATION_FAILED = -115, + /*! -116: AFBR-S50 Error: The VSUB calibration failed! */ + ERROR_ARGUS_VSUB_CALIBRATION_FAILED = -116, + /*! -191: AFBR-S50 Error: The device is currently busy and cannot execute the * requested command. */ ERROR_ARGUS_BUSY = -191, 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 a1a2d878ac99..f58ba1bba74e 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 @@ -56,13 +56,13 @@ extern "C" { #define ARGUS_API_VERSION_MAJOR 1 /*! Minor version number of the AFBR-S50 API. */ -#define ARGUS_API_VERSION_MINOR 4 +#define ARGUS_API_VERSION_MINOR 5 /*! Bugfix version number of the AFBR-S50 API. */ -#define ARGUS_API_VERSION_BUGFIX 4 +#define ARGUS_API_VERSION_BUGFIX 6 /*! Build version number of the AFBR-S50 API. */ -#define ARGUS_API_VERSION_BUILD "20230327150535" +#define ARGUS_API_VERSION_BUILD "20240208081753" /*****************************************************************************/ 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 6f3d40b49a42..284538a52bc9 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 @@ -72,30 +72,28 @@ typedef struct xtalk_t { * @details Contains crosstalk vector values for all 32 active pixels, * separated for A/B-Frames. *****************************************************************************/ -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]; +typedef union argus_cal_xtalk_table_t { + 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. + * @brief Electrical Pixel-To-Pixel Crosstalk Compensation Parameters. + * @details Contains calibration data that belongs to the electrical + * pixel-to-pixel crosstalk compensation feature. *****************************************************************************/ -typedef struct argus_cal_p2pxtalk_t { - /*! Pixel-To-Pixel Compensation on/off. */ +typedef struct argus_cal_electrical_p2pxtalk_t { + /*! Electrical Pixel-To-Pixel Compensation on/off. */ bool Enabled; /*! The relative threshold determines when the compensation is active for @@ -134,8 +132,39 @@ typedef struct argus_cal_p2pxtalk_t { * Higher values determine more influence on the reference pixel signal. */ q3_12_t KcFactorCRefPx; -} argus_cal_p2pxtalk_t; +} argus_cal_electrical_p2pxtalk_t; + +/*!*************************************************************************** + * @brief Optical Pixel-To-Pixel Crosstalk Compensation Parameters. + * @details Contains calibration data that belongs to the optical + * pixel-to-pixel crosstalk compensation feature. + *****************************************************************************/ +typedef struct argus_cal_optical_p2pxtalk_t { + /*! Optical Pixel-To-Pixel Compensation on/off. */ + bool Enabled; + + /*! The sine component of the coupling coefficient that determines the amount + * of a neighbour pixel signal that influences the raw signal of certain pixel. + * Higher values determine more influence on the individual pixel signal. */ + q3_12_t CouplingCoeffS; + + /*! The cosine component of the coupling coefficient that determines the amount + * of a neighbour pixel signal that influences the raw signal of a certain pixel. + * Higher values determine more influence on the individual pixel signal. */ + q3_12_t CouplingCoeffC; + +} argus_cal_optical_p2pxtalk_t; + +/*!*************************************************************************** + * @brief Pixel-To-Pixel Crosstalk Compensation Parameters. + * @details Contains combined calibration data for electrical and + * optical pixel-to-pixel crosstalk compensation feature. + *****************************************************************************/ +typedef struct argus_cal_p2pxtalk_t { + argus_cal_electrical_p2pxtalk_t Electrical; + argus_cal_optical_p2pxtalk_t Optical; +} argus_cal_p2pxtalk_t; /*! @} */ #ifdef __cplusplus 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 index 60b75a164a3e..09c6fdfdc3d7 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_div.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_div.h @@ -61,7 +61,7 @@ extern "C" { * @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. + * Division by 0 yields max. values determined by signs of numerator. * Too high/low results are truncated to max/min values. * * Depending on the architecture, the division is implemented with a 64-bit @@ -89,14 +89,14 @@ inline int32_t fp_div16(int32_t a, q15_16_t b) if (c > 0x80000000U) { return INT32_MIN; } - return -c; + return (int32_t) - c; } else { c = ((c / b) + (1 << 13U)) >> 14U; if (c > (int64_t)INT32_MAX) { return INT32_MAX; } - return c; + return (int32_t)c; } #else @@ -159,10 +159,16 @@ inline int32_t fp_div16(int32_t a, q15_16_t b) /* Figure out the sign of result */ if ((uint32_t)(a ^ b) & 0x80000000U) { - result = -result; + return (int32_t) - result; + + } else { + // fix 05.10.2023; the corner case, when result == INT32_MAX + 1: + // Catch the wraparound (to INT32_MIN) and truncate instead. + if (quotient > INT32_MAX) { return INT32_MAX; } + + return (int32_t)result; } - return (int32_t)result; #endif } 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 index 78db5826445a..e0996d6d78a5 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_mul.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_mul.h @@ -118,7 +118,7 @@ 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); + return (uint32_t)((w >> shift) + ((w >> (shift - 1)) & 1U)); #else uint32_t tmp[2] = { 0 }; muldwu(tmp, u, v); @@ -158,15 +158,15 @@ inline int32_t fp_muls(int32_t u, int32_t v, uint_fast8_t shift) uint32_t u2, v2; - if (u < 0) { u2 = -u; sign = -sign; } else { u2 = u; } + if (u < 0) { u2 = (uint32_t) - u; sign = -sign; } else { u2 = (uint32_t)u; } - if (v < 0) { v2 = -v; sign = -sign; } else { v2 = v; } + if (v < 0) { v2 = (uint32_t) - v; sign = -sign; } else { v2 = (uint32_t)v; } const uint32_t res = fp_mulu(u2, v2, shift); assert(sign > 0 ? res <= 0x7FFFFFFFU : res <= 0x80000000U); - return sign > 0 ? res : -res; + return sign > 0 ? (int32_t)res : -(int32_t)res; } @@ -225,7 +225,9 @@ inline uint32_t fp_mul_u32_u16(uint32_t u, uint16_t v, uint_fast8_t shift) *****************************************************************************/ 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); + return u >= 0 ? + (int32_t)fp_mul_u32_u16((uint32_t)u, v, shift) : + -(int32_t)fp_mul_u32_u16((uint32_t) - u, v, shift); } /*! @} */ 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 index ad6f71e09cc6..056f2e027df2 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_rnd.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_rnd.h @@ -80,7 +80,7 @@ inline uint32_t fp_rndu(uint32_t Q, uint_fast8_t n) *****************************************************************************/ inline int32_t fp_rnds(int32_t Q, uint_fast8_t n) { - return (Q < 0) ? -fp_rndu(-Q, n) : fp_rndu(Q, n); + return (Q < 0) ? -(int32_t)fp_rndu((uint32_t)(-Q), n) : (int32_t)fp_rndu((uint32_t)Q, n); } /*!*************************************************************************** @@ -108,7 +108,7 @@ inline uint32_t fp_truncu(uint32_t Q, uint_fast8_t n) *****************************************************************************/ inline int32_t fp_truncs(int32_t Q, uint_fast8_t n) { - return (Q < 0) ? -fp_truncu(-Q, n) : fp_truncu(Q, n); + return (Q < 0) ? -(int32_t)fp_truncu((uint32_t)(-Q), n) : (int32_t)fp_truncu((uint32_t)Q, n); } /*! @} */ 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 index 27de8cc68897..b9b200414663 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/int_math.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/int_math.h @@ -66,7 +66,7 @@ inline uint32_t log2i(uint32_t x) { assert(x != 0); #if 1 - return 31 - __builtin_clz(x); + return (uint32_t)(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; 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 e4e6dc0c52cd..1cd0fe27e0df 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 23bcd6fbea11..a6e7c581d6d4 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/cm8jl65/CM8JL65.cpp b/src/drivers/distance_sensor/cm8jl65/CM8JL65.cpp index 8887408ff305..89f71ee1c0b7 100644 --- a/src/drivers/distance_sensor/cm8jl65/CM8JL65.cpp +++ b/src/drivers/distance_sensor/cm8jl65/CM8JL65.cpp @@ -113,7 +113,7 @@ CM8JL65::CM8JL65(const char *port, uint8_t rotation) : _px4_rangefinder.set_max_distance(7.9f); // Datasheet: 8.0m _px4_rangefinder.set_fov(0.0488692f); _px4_rangefinder.set_device_type(DRV_DIST_DEVTYPE_CM8JL65); - _px4_rangefinder.set_rangefinder_type(distance_sensor_s::MAV_DISTANCE_SENSOR_LASER); + _px4_rangefinder.set_rangefinder_type(distance_sensor_s::MAV_DISTANCE_SENSOR_INFRARED); } CM8JL65::~CM8JL65() diff --git a/src/drivers/drv_hrt.h b/src/drivers/drv_hrt.h index 3e295abfc949..bb62da0189c3 100644 --- a/src/drivers/drv_hrt.h +++ b/src/drivers/drv_hrt.h @@ -162,7 +162,16 @@ static inline void abstime_to_ts(struct timespec *ts, hrt_abstime abstime) */ static inline hrt_abstime hrt_elapsed_time(const hrt_abstime *then) { - return hrt_absolute_time() - *then; + hrt_abstime now = hrt_absolute_time(); + + // Cannot allow a negative elapsed time as this would appear + // to be a huge positive elapsed time when represented as an + // unsigned value! + if (*then > now) { + return 0; + } + + return now - *then; } /** diff --git a/src/drivers/drv_orb_dev.h b/src/drivers/drv_orb_dev.h index 042dbe2097e1..42f361d43968 100644 --- a/src/drivers/drv_orb_dev.h +++ b/src/drivers/drv_orb_dev.h @@ -64,13 +64,10 @@ /** Get the priority for the topic */ #define ORBIOCGPRIORITY _ORBIOC(14) -/** Set the queue size of the topic */ -#define ORBIOCSETQUEUESIZE _ORBIOC(15) - /** Get the minimum interval at which the topic can be seen to be updated for this subscription */ -#define ORBIOCGETINTERVAL _ORBIOC(16) +#define ORBIOCGETINTERVAL _ORBIOC(15) /** Check whether the topic is advertised, sets *(unsigned long *)arg to 1 if advertised, 0 otherwise */ -#define ORBIOCISADVERTISED _ORBIOC(17) +#define ORBIOCISADVERTISED _ORBIOC(16) #endif /* _DRV_UORB_H */ diff --git a/src/drivers/gps/devices b/src/drivers/gps/devices index 63990d218ec3..17c0e2bfad1e 160000 --- a/src/drivers/gps/devices +++ b/src/drivers/gps/devices @@ -1 +1 @@ -Subproject commit 63990d218ec35ea965d634af6a79d3155561b743 +Subproject commit 17c0e2bfad1e544c4b11329c742630a765c7537f diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 4ed29a8c6e35..766be323d719 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -45,7 +45,6 @@ #include #endif -#include #include #include @@ -57,6 +56,8 @@ #include #include #include +#include +#include #include #include #include @@ -81,6 +82,7 @@ #include #endif /* __PX4_LINUX */ +using namespace device; using namespace time_literals; #define TIMEOUT_1HZ 1300 //!< Timeout time in mS, 1000 mS (1Hz) + 300 mS delta for error @@ -169,7 +171,10 @@ class GPS : public ModuleBase, public device::Device void reset_if_scheduled(); private: - int _serial_fd{-1}; ///< serial interface to GPS +#ifdef __PX4_LINUX + int _spi_fd {-1}; ///< SPI interface to GPS +#endif + Serial *_uart = nullptr; ///< UART interface to GPS unsigned _baudrate{0}; ///< current baudrate const unsigned _configured_baudrate{0}; ///< configured baudrate (0=auto-detect) char _port[20] {}; ///< device / serial port path @@ -329,8 +334,11 @@ GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interfac char c = _port[strlen(_port) - 1]; // last digit of path (eg /dev/ttyS2) set_device_bus(c - 48); // sub 48 to convert char to integer +#ifdef __PX4_LINUX + } else if (_interface == GPSHelper::Interface::SPI) { set_device_bus_type(device::Device::DeviceBusType::DeviceBusType_SPI); +#endif } if (_mode == gps_driver_mode_t::None) { @@ -403,10 +411,23 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user) return num_read; } - case GPSCallbackType::writeDeviceData: - gps->dumpGpsData((uint8_t *)data1, (size_t)data2, gps_dump_comm_mode_t::Full, true); + case GPSCallbackType::writeDeviceData: { + gps->dumpGpsData((uint8_t *)data1, (size_t)data2, gps_dump_comm_mode_t::Full, true); + + int ret = 0; + + if (gps->_uart) { + ret = gps->_uart->write((void *) data1, (size_t) data2); + +#ifdef __PX4_LINUX + + } else if (gps->_spi_fd >= 0) { + ret = ::write(gps->_spi_fd, data1, (size_t)data2); +#endif + } - return ::write(gps->_serial_fd, data1, (size_t)data2); + return ret; + } case GPSCallbackType::setBaudrate: return gps->setBaudrate(data2); @@ -449,72 +470,64 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user) int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout) { + int ret = 0; + const unsigned character_count = 32; // minimum bytes that we want to read + const int max_timeout = 50; + int timeout_adjusted = math::min(max_timeout, timeout); + handleInjectDataTopic(); -#if !defined(__PX4_QURT) + if ((_interface == GPSHelper::Interface::UART) && (_uart)) { + ret = _uart->readAtLeast(buf, buf_length, character_count, timeout_adjusted); - /* For non QURT, use the usual polling. */ +// SPI is only supported on LInux +#if defined(__PX4_LINUX) - //Poll only for the serial data. In the same thread we also need to handle orb messages, - //so ideally we would poll on both, the serial fd and orb subscription. Unfortunately the - //two pollings use different underlying mechanisms (at least under posix), which makes this - //impossible. Instead we limit the maximum polling interval and regularly check for new orb - //messages. - //FIXME: add a unified poll() API - const int max_timeout = 50; + } else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd >= 0)) { - pollfd fds[1]; - fds[0].fd = _serial_fd; - fds[0].events = POLLIN; - - int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), math::min(max_timeout, timeout)); - - if (ret > 0) { - /* if we have new data from GPS, go handle it */ - if (fds[0].revents & POLLIN) { - /* - * We are here because poll says there is some data, so this - * won't block even on a blocking device. But don't read immediately - * by 1-2 bytes, wait for some more data to save expensive read() calls. - * If we have all requested data available, read it without waiting. - * If more bytes are available, we'll go back to poll() again. - */ - const unsigned character_count = 32; // minimum bytes that we want to read - unsigned baudrate = _baudrate == 0 ? 115200 : _baudrate; - const unsigned sleeptime = character_count * 1000000 / (baudrate / 10); + //Poll only for the SPI data. In the same thread we also need to handle orb messages, + //so ideally we would poll on both, the SPI fd and orb subscription. Unfortunately the + //two pollings use different underlying mechanisms (at least under posix), which makes this + //impossible. Instead we limit the maximum polling interval and regularly check for new orb + //messages. + //FIXME: add a unified poll() API -#ifdef __PX4_NUTTX - int err = 0; - int bytes_available = 0; - err = ::ioctl(_serial_fd, FIONREAD, (unsigned long)&bytes_available); + pollfd fds[1]; + fds[0].fd = _spi_fd; + fds[0].events = POLLIN; + + ret = poll(fds, sizeof(fds) / sizeof(fds[0]), timeout_adjusted); + + if (ret > 0) { + /* if we have new data from GPS, go handle it */ + if (fds[0].revents & POLLIN) { + /* + * We are here because poll says there is some data, so this + * won't block even on a blocking device. But don't read immediately + * by 1-2 bytes, wait for some more data to save expensive read() calls. + * If we have all requested data available, read it without waiting. + * If more bytes are available, we'll go back to poll() again. + */ + unsigned baudrate = _baudrate == 0 ? 115200 : _baudrate; + const unsigned sleeptime = character_count * 1000000 / (baudrate / 10); - if (err != 0 || bytes_available < (int)character_count) { px4_usleep(sleeptime); - } -#else - px4_usleep(sleeptime); -#endif + ret = ::read(_spi_fd, buf, buf_length); - ret = ::read(_serial_fd, buf, buf_length); + if (ret > 0) { + _num_bytes_read += ret; + } - if (ret > 0) { - _num_bytes_read += ret; + } else { + ret = -1; } - - } else { - ret = -1; } + +#endif } return ret; - -#else - /* For QURT, just use read for now, since this doesn't block, we need to slow it down - * just a bit. */ - px4_usleep(10000); - return ::read(_serial_fd, buf, buf_length); -#endif } void GPS::handleInjectDataTopic() @@ -583,105 +596,38 @@ bool GPS::injectData(uint8_t *data, size_t len) { dumpGpsData(data, len, gps_dump_comm_mode_t::Full, true); - size_t written = ::write(_serial_fd, data, len); - ::fsync(_serial_fd); - return written == len; -} - -int GPS::setBaudrate(unsigned baud) -{ - /* process baud rate */ - int speed; - - switch (baud) { - case 9600: speed = B9600; break; - - case 19200: speed = B19200; break; - - case 38400: speed = B38400; break; - - case 57600: speed = B57600; break; - - case 115200: speed = B115200; break; - - case 230400: speed = B230400; break; + size_t written = 0; -#ifndef B460800 -#define B460800 460800 -#endif + if ((_interface == GPSHelper::Interface::UART) && (_uart)) { + written = _uart->write((const void *) data, len); - case 460800: speed = B460800; break; +#ifdef __PX4_LINUX -#ifndef B921600 -#define B921600 921600 + } else if (_interface == GPSHelper::Interface::SPI) { + written = ::write(_spi_fd, data, len); + ::fsync(_spi_fd); #endif - - case 921600: speed = B921600; break; - - default: - PX4_ERR("ERR: unknown baudrate: %d", baud); - return -EINVAL; } - struct termios uart_config; - - int termios_state; - - /* fill the struct for the new configuration */ - tcgetattr(_serial_fd, &uart_config); - - /* properly configure the terminal (see also https://en.wikibooks.org/wiki/Serial_Programming/termios ) */ - - // - // Input flags - Turn off input processing - // - // convert break to null byte, no CR to NL translation, - // no NL to CR translation, don't mark parity errors or breaks - // no input parity check, don't strip high bit off, - // no XON/XOFF software flow control - // - uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | - INLCR | PARMRK | INPCK | ISTRIP | IXON); - // - // Output flags - Turn off output processing - // - // no CR to NL translation, no NL to CR-NL translation, - // no NL to CR translation, no column 0 CR suppression, - // no Ctrl-D suppression, no fill characters, no case mapping, - // no local output processing - // - // config.c_oflag &= ~(OCRNL | ONLCR | ONLRET | - // ONOCR | ONOEOT| OFILL | OLCUC | OPOST); - uart_config.c_oflag = 0; - - // - // No line processing - // - // echo off, echo newline off, canonical mode off, - // extended input processing off, signal chars off - // - uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG); - - /* no parity, one stop bit, disable flow control */ - uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS); - - /* set baud rate */ - if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { - GPS_ERR("ERR: %d (cfsetispeed)", termios_state); - return -1; - } + return written == len; +} - if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { - GPS_ERR("ERR: %d (cfsetospeed)", termios_state); - return -1; - } +int GPS::setBaudrate(unsigned baud) +{ + if (_interface == GPSHelper::Interface::UART) { + if ((_uart) && (_uart->setBaudrate(baud))) { + return 0; + } + +#ifdef __PX4_LINUX - if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) { - GPS_ERR("ERR: %d (tcsetattr)", termios_state); - return -1; + } else if (_interface == GPSHelper::Interface::SPI) { + // Can't set the baudrate on a SPI port but just return a success + return 0; +#endif } - return 0; + return -1; } void GPS::initializeCommunicationDump() @@ -840,31 +786,58 @@ GPS::run() _helper = nullptr; } - if (_serial_fd < 0) { - /* open the serial port */ - _serial_fd = ::open(_port, O_RDWR | O_NOCTTY); + if ((_interface == GPSHelper::Interface::UART) && (_uart == nullptr)) { + + // Create the UART port instance + _uart = new Serial(_port); + + if (_uart == nullptr) { + PX4_ERR("Error creating serial device %s", _port); + px4_sleep(1); + continue; + } + } + + if ((_interface == GPSHelper::Interface::UART) && (! _uart->isOpen())) { + // Configure the desired baudrate if one was specified by the user. + // Otherwise the default baudrate will be used. + if (_configured_baudrate) { + if (! _uart->setBaudrate(_configured_baudrate)) { + PX4_ERR("Error setting baudrate to %u on %s", _configured_baudrate, _port); + px4_sleep(1); + continue; + } + } - if (_serial_fd < 0) { - PX4_ERR("failed to open %s err: %d", _port, errno); + // Open the UART. If this is successful then the UART is ready to use. + if (! _uart->open()) { + PX4_ERR("Error opening serial device %s", _port); px4_sleep(1); continue; } #ifdef __PX4_LINUX - if (_interface == GPSHelper::Interface::SPI) { - int spi_speed = 1000000; // make sure the bus speed is not too high (required on RPi) - int status_value = ::ioctl(_serial_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed); + } else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd < 0)) { + _spi_fd = ::open(_port, O_RDWR | O_NOCTTY); - if (status_value < 0) { - PX4_ERR("SPI_IOC_WR_MAX_SPEED_HZ failed for %s (%d)", _port, errno); - } + if (_spi_fd < 0) { + PX4_ERR("failed to open SPI port %s err: %d", _port, errno); + px4_sleep(1); + continue; + } - status_value = ::ioctl(_serial_fd, SPI_IOC_RD_MAX_SPEED_HZ, &spi_speed); + int spi_speed = 1000000; // make sure the bus speed is not too high (required on RPi) + int status_value = ::ioctl(_spi_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed); - if (status_value < 0) { - PX4_ERR("SPI_IOC_RD_MAX_SPEED_HZ failed for %s (%d)", _port, errno); - } + if (status_value < 0) { + PX4_ERR("SPI_IOC_WR_MAX_SPEED_HZ failed for %s (%d)", _port, errno); + } + + status_value = ::ioctl(_spi_fd, SPI_IOC_RD_MAX_SPEED_HZ, &spi_speed); + + if (status_value < 0) { + PX4_ERR("SPI_IOC_RD_MAX_SPEED_HZ failed for %s (%d)", _port, errno); } #endif /* __PX4_LINUX */ @@ -1056,9 +1029,17 @@ GPS::run() } } - if (_serial_fd >= 0) { - ::close(_serial_fd); - _serial_fd = -1; + if ((_interface == GPSHelper::Interface::UART) && (_uart)) { + (void) _uart->close(); + delete _uart; + _uart = nullptr; + +#ifdef __PX4_LINUX + + } else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd >= 0)) { + ::close(_spi_fd); + _spi_fd = -1; +#endif } if (_mode_auto) { @@ -1406,7 +1387,7 @@ int GPS::task_spawn(int argc, char *argv[], Instance instance) entry_point, (char *const *)argv); if (task_id < 0) { - task_id = -1; + _task_id = -1; return -errno; } @@ -1477,12 +1458,12 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance) break; case 'i': - if (!strcmp(myoptarg, "spi")) { - interface = GPSHelper::Interface::SPI; - - } else if (!strcmp(myoptarg, "uart")) { + if (!strcmp(myoptarg, "uart")) { interface = GPSHelper::Interface::UART; - +#ifdef __PX4_LINUX + } else if (!strcmp(myoptarg, "spi")) { + interface = GPSHelper::Interface::SPI; +#endif } else { PX4_ERR("unknown interface: %s", myoptarg); error_flag = true; @@ -1490,12 +1471,12 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance) break; case 'j': - if (!strcmp(myoptarg, "spi")) { - interface_secondary = GPSHelper::Interface::SPI; - - } else if (!strcmp(myoptarg, "uart")) { + if (!strcmp(myoptarg, "uart")) { interface_secondary = GPSHelper::Interface::UART; - +#ifdef __PX4_LINUX + } else if (!strcmp(myoptarg, "spi")) { + interface_secondary = GPSHelper::Interface::SPI; +#endif } else { PX4_ERR("unknown interface for secondary: %s", myoptarg); error_flag = true; diff --git a/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp b/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp index f8c938a5cdc3..a2fc22a9f2e0 100644 --- a/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp +++ b/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp @@ -88,7 +88,7 @@ class ICM42688P : public device::SPI, public I2CSPIDriver FIFO::DATA f[FIFO_MAX_SAMPLES] {}; }; // ensure no struct padding - static_assert(sizeof(FIFOTransferBuffer) == (4 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA))); + static_assert(sizeof(FIFOTransferBuffer) == (4 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)), "Invalid transfer buffer size"); struct register_bank0_config_t { Register::BANK_0 reg; diff --git a/src/drivers/optical_flow/thoneflow/thoneflow_parser.cpp b/src/drivers/optical_flow/thoneflow/thoneflow_parser.cpp index 3534af002abb..54cc21d105a3 100644 --- a/src/drivers/optical_flow/thoneflow/thoneflow_parser.cpp +++ b/src/drivers/optical_flow/thoneflow/thoneflow_parser.cpp @@ -133,8 +133,8 @@ bool thoneflow_parse(char c, char *parserbuf, unsigned *parserbuf_index, enum TH // Checksum valid, populate sensor report int16_t delta_x = uint16_t(parserbuf[1]) << 8 | parserbuf[0]; int16_t delta_y = uint16_t(parserbuf[3]) << 8 | parserbuf[2]; - flow->pixel_flow[0] = static_cast(delta_x) * (3.52e-3f); - flow->pixel_flow[1] = static_cast(delta_y) * (3.52e-3f); + flow->pixel_flow[0] = static_cast(delta_x) * (1.76e-3f); + flow->pixel_flow[1] = static_cast(delta_y) * (1.76e-3f); *state = THONEFLOW_PARSE_STATE7_CHECKSUM; } else { diff --git a/src/drivers/power_monitor/ina238/ina238.cpp b/src/drivers/power_monitor/ina238/ina238.cpp index d8afc1bd636b..6d58e84d8f95 100644 --- a/src/drivers/power_monitor/ina238/ina238.cpp +++ b/src/drivers/power_monitor/ina238/ina238.cpp @@ -66,6 +66,15 @@ INA238::INA238(const I2CSPIDriverConfig &config, int battery_index) : } _current_lsb = _max_current / INA238_DN_MAX; + _shunt_calibration = static_cast(INA238_CONST * _current_lsb * _rshunt); + + if (_range == INA238_ADCRANGE_LOW) { + _shunt_calibration *= 4; + } + + _register_cfg[0].set_bits = (uint16_t)(_range); + _register_cfg[2].set_bits = _shunt_calibration; + _register_cfg[2].clear_bits = ~_shunt_calibration; // We need to publish immediately, to guarantee that the first instance of the driver publishes to uORB instance 0 _battery.setConnected(false); @@ -114,33 +123,7 @@ int INA238::init() return ret; } - if (write(INA238_REG_CONFIG, (uint16_t)(INA238_RST_RESET | _range)) != PX4_OK) { - return ret; - } - - uint16_t shunt_calibration = static_cast(INA238_CONST * _current_lsb * _rshunt); - - if (_range == INA238_ADCRANGE_LOW) { - shunt_calibration *= 4; - } - - if (write(INA238_REG_SHUNTCAL, shunt_calibration) < 0) { - return -3; - } - - // Set the CONFIG for max I - if (write(INA238_REG_CONFIG, (uint16_t) _range) != PX4_OK) { - return ret; - } - - // Start ADC continous mode here - ret = write(INA238_REG_ADCCONFIG, (uint16_t)INA238_ADCCONFIG); - - start(); - _sensor_ok = true; - - _initialized = ret == PX4_OK; - return ret; + return Reset(); } int INA238::force_init() @@ -156,12 +139,12 @@ int INA238::probe() { uint16_t value{0}; - if (read(INA238_MANUFACTURER_ID, value) != PX4_OK || value != INA238_MFG_ID_TI) { + if (RegisterRead(Register::MANUFACTURER_ID, value) != PX4_OK || value != INA238_MFG_ID_TI) { PX4_DEBUG("probe mfgid %d", value); return -1; } - if (read(INA238_DEVICE_ID, value) != PX4_OK || ( + if (RegisterRead(Register::DEVICE_ID, value) != PX4_OK || ( INA238_DEVICEID(value) != INA238_MFG_DIE )) { PX4_DEBUG("probe die id %d", value); @@ -171,6 +154,58 @@ int INA238::probe() return PX4_OK; } +int INA238::Reset() +{ + + int ret = PX4_ERROR; + + if (RegisterWrite(Register::CONFIG, (uint16_t)(ADC_RESET_BIT)) != PX4_OK) { + return ret; + } + + if (RegisterWrite(Register::SHUNT_CAL, uint16_t(_shunt_calibration)) < 0) { + return -3; + } + + // Set the CONFIG for max I + if (RegisterWrite(Register::CONFIG, (uint16_t) _range) != PX4_OK) { + return ret; + } + + // Start ADC continous mode here + ret = write((uint16_t)_register_cfg[1].reg, (uint16_t)_register_cfg[1].set_bits); + + return ret; +} + +bool INA238::RegisterCheck(const register_config_t ®_cfg) +{ + bool success = true; + + uint16_t reg_value = 0; + RegisterRead(reg_cfg.reg, reg_value); + + 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; +} + +int INA238::RegisterWrite(Register reg, uint16_t value) +{ + return write((uint8_t)reg, value); +} +int INA238::RegisterRead(Register reg, uint16_t &value) +{ + return read((uint8_t)reg, value); +} int INA238::collect() { @@ -190,8 +225,22 @@ int INA238::collect() int16_t bus_voltage{0}; int16_t current{0}; - success = success && (read(INA238_REG_VSBUS, bus_voltage) == PX4_OK); - success = success && (read(INA238_REG_CURRENT, current) == PX4_OK); + success = (RegisterRead(Register::VS_BUS, (uint16_t &)bus_voltage) == PX4_OK); + success = success && (RegisterRead(Register::CURRENT, (uint16_t &)current) == PX4_OK); + + if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { + // check configuration registers periodically or immediately following any failure + if (RegisterCheck(_register_cfg[_checked_register])) { + _last_config_check_timestamp = hrt_absolute_time(); + _checked_register = (_checked_register + 1) % size_register_cfg; + + } else { + // register check failed, force reset + PX4_DEBUG("register check failed"); + perf_count(_bad_register_perf); + success = false; + } + } if (!success) { PX4_DEBUG("error reading from sensor"); @@ -234,7 +283,9 @@ void INA238::RunImpl() if (collect() != PX4_OK) { perf_count(_collection_errors); /* if error restart the measurement state machine */ - start(); + ScheduleClear(); + _initialized = false; + ScheduleNow(); return; } @@ -262,6 +313,10 @@ void INA238::RunImpl() if (init() != PX4_OK) { ScheduleDelayed(INA238_INIT_RETRY_INTERVAL_US); + + } else { + _initialized = true; + start(); } } } diff --git a/src/drivers/power_monitor/ina238/ina238.h b/src/drivers/power_monitor/ina238/ina238.h index a9108c258eab..c40514ad58a7 100644 --- a/src/drivers/power_monitor/ina238/ina238.h +++ b/src/drivers/power_monitor/ina238/ina238.h @@ -44,8 +44,10 @@ #include #include #include +#include "ina238_registers.hpp" using namespace time_literals; +using namespace ina238; /* Configuration Constants */ #define INA238_BASEADDR 0x45 /* 7-bit address. 8-bit address is 0x45 */ @@ -53,225 +55,14 @@ using namespace time_literals; // connect to the INA238 every this many microseconds #define INA238_INIT_RETRY_INTERVAL_US 500000 -/* INA238 Registers addresses */ -#define INA238_REG_CONFIG (0x00) -#define INA238_REG_ADCCONFIG (0x01) -#define INA238_REG_SHUNTCAL (0x02) -#define INA238_REG_SHUNTTEMPCO (0x03) -#define INA238_REG_VSHUNT (0x04) -#define INA238_REG_VSBUS (0x05) -#define INA238_REG_DIETEMP (0x06) -#define INA238_REG_CURRENT (0x07) -#define INA238_REG_POWER (0x08) -#define INA238_REG_ENERGY (0x09) -#define INA238_REG_CHARGE (0x0a) -#define INA238_REG_DIAG_ALRT (0x0b) -#define INA238_REG_SOVL (0x0c) -#define INA238_REG_SUVL (0x0d) -#define INA238_REG_BOVL (0x0e) -#define INA238_REG_BUVL (0x0f) -#define INA238_REG_TEMP_LIMIT (0x10) -#define INA238_REG_TPWR_LIMIT (0x11) -#define INA238_MANUFACTURER_ID (0x3e) -#define INA238_DEVICE_ID (0x3f) #define INA238_MFG_ID_TI (0x5449) // TI #define INA238_MFG_DIE (0x238) // INA237, INA238 -/* INA238 Configuration (CONFIG) 16-bit Register (Address = 0h) [reset = 0h] */ #define INA238_ADCRANGE_SHIFTS (4) -#define INA238_ADCRANGE_MASK (1 << INA238_ADCRANGE_SHIFTS) #define INA238_ADCRANGE_LOW (1 << INA238_ADCRANGE_SHIFTS) // ± 40.96 mV #define INA238_ADCRANGE_HIGH (0 << INA238_ADCRANGE_SHIFTS) // ±163.84 mV -#define INA238_TEMPCOMP_SHIFTS (5) -#define INA238_TEMPCOMP_MASK (1 << INA238_TEMPCOMP_SHIFTS) -#define INA238_TEMPCOMP_ENABLE (1 << INA238_TEMPCOMP_SHIFTS) -#define INA238_TEMPCOMP_DISABLE (0 << INA238_TEMPCOMP_SHIFTS) - -#define INA238_CONVDLY_SHIFTS (6) -#define INA238_CONVDLY_MASK (0xff << INA238_CONVDLY_SHIFTS) -#define INA238_CONVDLY2MS(n) ((n) << INA238_CONVDLY_SHIFTS) - -#define INA238_RSTACC_SHIFTS (14) -#define INA238_RSTACC_MASK (1 << INA238_RSTACC_SHIFTS) -#define INA238_RSTACC_CLEAR (1 << INA238_RSTACC_SHIFTS) -#define INA238_RSTACC_NORMAL (0 << INA238_RSTACC_SHIFTS) - -#define INA238_RST_SHIFTS (15) -#define INA238_RST_MASK (1 << INA238_RST_SHIFTS) -#define INA238_RST_RESET (1 << INA238_RST_SHIFTS) -#define INA238_RST_NORMAL (0 << INA238_RST_SHIFTS) - -/* INA238 ADC Configuration (ADC_CONFIG) 16-bit Register (Address = 1h) [reset = FB68h] */ -#define INA238_MODE_SHIFTS (12) -#define INA238_MODE_MASK (0xf << INA238_MODE_SHIFTS) -#define INA238_MODE_SHUTDOWN_TRIG (0 << INA238_MODE_SHIFTS) -#define INA238_MODE_BUS_TRIG (1 << INA238_MODE_SHIFTS) -#define INA238_MODE_SHUNT_TRIG (2 << INA238_MODE_SHIFTS) -#define INA238_MODE_SHUNT_BUS_TRIG (3 << INA238_MODE_SHIFTS) -#define INA238_MODE_TEMP_TRIG (4 << INA238_MODE_SHIFTS) -#define INA238_MODE_TEMP_BUS_TRIG (5 << INA238_MODE_SHIFTS) -#define INA238_MODE_TEMP_SHUNT_TRIG (6 << INA238_MODE_SHIFTS) -#define INA238_MODE_TEMP_SHUNT_BUS_TRIG (7 << INA238_MODE_SHIFTS) - -#define INA238_MODE_SHUTDOWN_CONT (8 << INA238_MODE_SHIFTS) -#define INA238_MODE_BUS_CONT (9 << INA238_MODE_SHIFTS) -#define INA238_MODE_SHUNT_CONT (10 << INA238_MODE_SHIFTS) -#define INA238_MODE_SHUNT_BUS_CONT (11 << INA238_MODE_SHIFTS) -#define INA238_MODE_TEMP_CONT (12 << INA238_MODE_SHIFTS) -#define INA238_MODE_TEMP_BUS_CONT (13 << INA238_MODE_SHIFTS) -#define INA238_MODE_TEMP_SHUNT_CONT (14 << INA238_MODE_SHIFTS) -#define INA238_MODE_TEMP_SHUNT_BUS_CONT (15 << INA238_MODE_SHIFTS) - -#define INA238_VBUSCT_SHIFTS (9) -#define INA238_VBUSCT_MASK (7 << INA238_VBUSCT_SHIFTS) -#define INA238_VBUSCT_50US (0 << INA238_VBUSCT_SHIFTS) -#define INA238_VBUSCT_84US (1 << INA238_VBUSCT_SHIFTS) -#define INA238_VBUSCT_150US (2 << INA238_VBUSCT_SHIFTS) -#define INA238_VBUSCT_280US (3 << INA238_VBUSCT_SHIFTS) -#define INA238_VBUSCT_540US (4 << INA238_VBUSCT_SHIFTS) -#define INA238_VBUSCT_1052US (5 << INA238_VBUSCT_SHIFTS) -#define INA238_VBUSCT_2074US (6 << INA238_VBUSCT_SHIFTS) -#define INA238_VBUSCT_4170US (7 << INA238_VBUSCT_SHIFTS) - -#define INA238_VSHCT_SHIFTS (6) -#define INA238_VSHCT_MASK (7 << INA238_VSHCT_SHIFTS) -#define INA238_VSHCT_50US (0 << INA238_VSHCT_SHIFTS) -#define INA238_VSHCT_84US (1 << INA238_VSHCT_SHIFTS) -#define INA238_VSHCT_150US (2 << INA238_VSHCT_SHIFTS) -#define INA238_VSHCT_280US (3 << INA238_VSHCT_SHIFTS) -#define INA238_VSHCT_540US (4 << INA238_VSHCT_SHIFTS) -#define INA238_VSHCT_1052US (5 << INA238_VSHCT_SHIFTS) -#define INA238_VSHCT_2074US (6 << INA238_VSHCT_SHIFTS) -#define INA238_VSHCT_4170US (7 << INA238_VSHCT_SHIFTS) - -#define INA238_VTCT_SHIFTS (3) -#define INA238_VTCT_MASK (7 << INA238_VTCT_SHIFTS) -#define INA238_VTCT_50US (0 << INA238_VTCT_SHIFTS) -#define INA238_VTCT_84US (1 << INA238_VTCT_SHIFTS) -#define INA238_VTCT_150US (2 << INA238_VTCT_SHIFTS) -#define INA238_VTCT_280US (3 << INA238_VTCT_SHIFTS) -#define INA238_VTCT_540US (4 << INA238_VTCT_SHIFTS) -#define INA238_VTCT_1052US (5 << INA238_VTCT_SHIFTS) -#define INA238_VTCT_2074US (6 << INA238_VTCT_SHIFTS) -#define INA238_VTCT_4170US (7 << INA238_VTCT_SHIFTS) - -#define INA238_AVERAGES_SHIFTS (0) -#define INA238_AVERAGES_MASK (7 << INA238_AVERAGES_SHIFTS) -#define INA238_AVERAGES_1 (0 << INA238_AVERAGES_SHIFTS) -#define INA238_AVERAGES_4 (1 << INA238_AVERAGES_SHIFTS) -#define INA238_AVERAGES_16 (2 << INA238_AVERAGES_SHIFTS) -#define INA238_AVERAGES_64 (3 << INA238_AVERAGES_SHIFTS) -#define INA238_AVERAGES_128 (4 << INA238_AVERAGES_SHIFTS) -#define INA238_AVERAGES_256 (5 << INA238_AVERAGES_SHIFTS) -#define INA238_AVERAGES_512 (6 << INA238_AVERAGES_SHIFTS) -#define INA238_AVERAGES_1024 (7 << INA238_AVERAGES_SHIFTS) - -#define INA238_ADCCONFIG (INA238_MODE_TEMP_SHUNT_BUS_CONT | INA238_VBUSCT_540US | INA238_VSHCT_540US | INA238_VTCT_540US |INA238_AVERAGES_64) -/* INA238 Shunt Calibration (SHUNT_CAL) 16-bit Register (Address = 2h) [reset = 1000h] */ - -#define INA238_CURRLSB_SHIFTS (0) -#define INA238_CURRLSB_MASK (0x7fff << INA238_CURRLSB_SHIFTS) - -/* INA238 Shunt Temperature Coefficient (SHUNT_TEMPCO) 16-bit Register (Address = 3h) [reset = 0h] */ - -#define INA238_TEMPCO_SHIFTS (0) -#define INA238_TEMPCO_MASK (0x1fff << INA238_TEMPCO_SHIFTS) - -/* INA238 Shunt Voltage Measurement (VSHUNT) 24-bit Register (Address = 4h) [reset = 0h] */ - -#define INA238_VSHUNT_SHIFTS (4) -#define INA238_VSHUNT_MASK (UINT32_C(0xffffff) << INA238_VSHUNT_SHIFTS) - -/* INA238 Bus Voltage Measurement (VBUS) 24-bit Register (Address = 5h) [reset = 0h] */ - -#define INA238_VBUS_SHIFTS (4) -#define INA238_VBUS_MASK (UINT32_C(0xffffff) << INA238_VBUS_SHIFTS) - -/* INA238 Temperature Measurement (DIETEMP) 16-bit Register (Address = 6h) [reset = 0h] */ - -#define INA238_DIETEMP_SHIFTS (0) -#define INA238_DIETEMP_MASK (0xffff << INA238_DIETEMP_SHIFTS) - -/* INA238 Current Result (CURRENT) 24-bit Register (Address = 7h) [reset = 0h] */ - -#define INA238_CURRENT_SHIFTS (4) -#define INA238_CURRENT_MASK (UINT32_C(0xffffff) << INA238_CURRENT_SHIFTS) - -/* INA238 Power Result (POWER) 24-bit Register (Address = 8h) [reset = 0h] */ - -#define INA238_POWER_SHIFTS (0) -#define INA238_POWER_MASK (UINT32_C(0xffffff) << INA238_POWER_SHIFTS) - -/* INA238 Energy Result (ENERGY) 40-bit Register (Address = 9h) [reset = 0h] */ - -#define INA238_ENERGY_SHIFTS (0) -#define INA238_ENERGY_MASK (UINT64_C(0xffffffffff) << INA238_ENERGY_SHIFTS) - -/* INA238 Energy Result (CHARGE) 40-bit Register (Address = Ah) [reset = 0h] */ - -#define INA238_CHARGE_SHIFTS (0) -#define INA238_CHARGE_MASK (UINT64_C(0xffffffffff) << INA238_CHARGE_SHIFTS) - - -/* INA238 Diagnostic Flags and Alert (DIAG_ALRT) 16-bit Register (Address = Bh) [reset = 0001h] */ - -#define INA238_MEMSTAT (1 << 0) // This bit is set to 0 if a checksum error is detected in the device trim memory space -#define INA238_CNVRF (1 << 1) // This bit is set to 1 if the conversion is completed. When ALATCH =1 this bit is cleared by reading the register or starting a new triggered conversion. -#define INA238_POL (1 << 2) // This bit is set to 1 if the power measurement exceeds the threshold limit in the power limit register. -#define INA238_BUSUL (1 << 3) // This bit is set to 1 if the bus voltage measurement falls below the threshold limit in the bus under-limit register. -#define INA238_BUSOL (1 << 4) // This bit is set to 1 if the bus voltage measurement exceeds the threshold limit in the bus over-limit register. -#define INA238_SHNTUL (1 << 5) // This bit is set to 1 if the shunt voltage measurement falls below the threshold limit in the shunt under-limit register -#define INA238_SHNTOL (1 << 6) // This bit is set to 1 if the shunt voltage measurement exceeds the threshold limit in the shunt over-limit register. -#define INA238_TMPOL (1 << 7) // This bit is set to 1 if the temperature measurement exceeds the threshold limit in the temperature over-limit register. -#define INA238_MATHOF (1 << 9) // This bit is set to 1 if an arithmetic operation resulted in an overflow error. -#define INA238_CHARGEOF (1 << 10) // This bit indicates the health of the CHARGE register. If the 40 bit CHARGE register has overflowed this bit is set to 1. -#define INA238_ENERGYOF (1 << 11) // This bit indicates the health of the ENERGY register. If the 40 bit ENERGY register has overflowed this bit is set to 1. -#define INA238_APOL (1 << 12) // Alert Polarity bit sets the Alert pin polarity. -#define INA238_SLOWALER (1 << 13) // ALERT function is asserted on the completed averaged value. This gives the flexibility to delay the ALERT after the averaged value. -#define INA238_CNVR (1 << 14) // Setting this bit high configures the Alert pin to be asserted when the Conversion Ready Flag (bit 1) is asserted, indicating that a conversion cycle has completed -#define INA238_ALATCH (1 << 15) // When the Alert Latch Enable bit is set to Transparent mode, the Alert pin and Flag bit reset to the idle state when the fault has been -// cleared. When the Alert Latch Enable bit is set to Latch mode, the Alert pin and Alert Flag bit remain active following a fault until -// the DIAG_ALRT Register has been read. - -/* Shunt Overvoltage Threshold (SOVL) 16-bit Register (Address = Ch) [reset = 7FFFh] */ - -#define INA238_SOVL_SHIFTS (0) -#define INA238_SOVL_MASK (0xffff << INA238_SOVL_SHIFTS) - -/* Shunt Undervoltage Threshold (SUVL) 16-bit Register (Address = Dh) [reset = 8000h] */ - -#define INA238_SUVL_SHIFTS (0) -#define INA238_SUVL_MASK (0xffff << INA238_SUVL_SHIFTS) - -/* Bus Overvoltage Threshold (BOVL) 16-bit Register (Address = Eh) [reset = 7FFFh] */ - -#define INA238_BOVL_SHIFTS (0) -#define INA238_BOVL_MASK (0xffff << INA238_BOVL_SHIFTS) - -/* Bus Undervoltage Threshold (BUVL) 16-bit Register (Address = Fh) [reset = 0h] */ - -#define INA238_BUVL_SHIFTS (0) -#define INA238_BUVL_MASK (0xffff << INA238_BUVL_SHIFTS) - -/* Temperature Over-Limit Threshold (TEMP_LIMIT) 16-bit Register (Address = 10h) [reset = 7FFFh */ - -#define INA238_TEMP_LIMIT_SHIFTS (0) -#define INA238_TEMP_LIMIT_MASK (0xffff << INA238_TEMP_LIMIT_SHIFTS) - -/* Power Over-Limit Threshold (PWR_LIMIT) 16-bit Register (Address = 11h) [reset = FFFFh] */ - -#define INA238_POWER_LIMIT_SHIFTS (0) -#define INA238_POWER_LIMIT_MASK (0xffff << INA238_POWER_LIMIT_SHIFTS) - -/* Manufacturer ID (MANUFACTURER_ID) 16-bit Register (Address = 3Eh) [reset = 5449h] */ - -/* Device ID (DEVICE_ID) 16-bit Register (Address = 3Fh) [reset = 2380h] */ - -#define INA238_DEVICE_REV_ID_SHIFTS (0) -#define INA238_DEVICE_REV_ID_MASK (0xf << INA238_DEVICE_REV_ID_SHIFTS) -#define INA238_DEVICEREV_ID(v) (((v) & INA238_DEVICE_REV_ID_MASK) >> INA238_DEVICE_REV_ID_SHIFTS) #define INA238_DEVICE_ID_SHIFTS (4) #define INA238_DEVICE_ID_MASK (0xfff << INA238_DEVICE_ID_SHIFTS) #define INA238_DEVICEID(v) (((v) & INA238_DEVICE_ID_MASK) >> INA238_DEVICE_ID_SHIFTS) @@ -283,7 +74,6 @@ using namespace time_literals; #define INA238_CONST 819.2e6f /* is an internal fixed value used to ensure scaling is maintained properly */ #define INA238_VSCALE 3.125e-03f /* LSB of voltage is 3.1255 mV/LSB */ - #define DEFAULT_MAX_CURRENT 327.68f /* Amps */ #define DEFAULT_SHUNT 0.0003f /* Shunt is 300 uOhm */ @@ -321,7 +111,17 @@ class INA238 : public device::I2C, public ModuleParams, public I2CSPIDriver= (int)OutputFunction::Servo1 && output_function <= (int)OutputFunction::ServoMax) { // Function got set to a servo int32_t val = 1500; - PX4_INFO("Setting channel %i disarmed to %i", (int) val, i); + PX4_INFO("Setting channel %i disarmed to %i", i + 1, (int)val); param_set(_mixing_output.disarmedParamHandle(i), &val); // If the whole timer group was not set previously, then set the pwm rate to 50 Hz @@ -250,7 +250,7 @@ void PWMOut::update_params() 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); + PX4_INFO("Setting timer %i to %i Hz", timer, (int)tim_config); param_set(handle, &tim_config); } } @@ -261,10 +261,10 @@ void PWMOut::update_params() if (output_function >= (int)OutputFunction::Motor1 && output_function <= (int)OutputFunction::MotorMax) { // Function got set to a motor int32_t val = 1100; - PX4_INFO("Setting channel %i minimum to %i", (int) val, i); + PX4_INFO("Setting channel %i minimum to %i", i + 1, (int)val); param_set(_mixing_output.minParamHandle(i), &val); val = 1900; - PX4_INFO("Setting channel %i maximum to %i", (int) val, i); + PX4_INFO("Setting channel %i maximum to %i", i + 1, (int)val); param_set(_mixing_output.maxParamHandle(i), &val); } } diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 12ab205c691e..bd6b5d1f3192 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -705,7 +705,7 @@ void PX4IO::update_params() if (output_function >= (int)OutputFunction::Servo1 && output_function <= (int)OutputFunction::ServoMax) { // Function got set to a servo int32_t val = 1500; - PX4_INFO("Setting channel %i disarmed to %i", (int) val, i); + PX4_INFO("Setting channel %i disarmed to %i", i + 1, (int)val); param_set(_mixing_output.disarmedParamHandle(i), &val); // If the whole timer group was not set previously, then set the pwm rate to 50 Hz @@ -726,7 +726,7 @@ void PX4IO::update_params() 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); + PX4_INFO("Setting timer %i to %i Hz", timer, (int)tim_config); param_set(handle, &tim_config); } } @@ -737,10 +737,10 @@ void PX4IO::update_params() if (output_function >= (int)OutputFunction::Motor1 && output_function <= (int)OutputFunction::MotorMax) { // Function got set to a motor int32_t val = 1100; - PX4_INFO("Setting channel %i minimum to %i", (int) val, i); + PX4_INFO("Setting channel %i minimum to %i", i + 1, (int)val); param_set(_mixing_output.minParamHandle(i), &val); val = 1900; - PX4_INFO("Setting channel %i maximum to %i", (int) val, i); + PX4_INFO("Setting channel %i maximum to %i", i + 1, (int)val); param_set(_mixing_output.maxParamHandle(i), &val); } } diff --git a/src/drivers/rc_input/RCInput.cpp b/src/drivers/rc_input/RCInput.cpp index 5caab7e97388..bea3af194c16 100644 --- a/src/drivers/rc_input/RCInput.cpp +++ b/src/drivers/rc_input/RCInput.cpp @@ -302,6 +302,25 @@ void RCInput::rc_io_invert(bool invert) } } +void RCInput::swap_rx_tx() +{ +#if defined(RC_SERIAL_SWAP_USING_SINGLEWIRE) + int rv = -ENOTTY; +# if defined(TIOCSSWAP) + rv = ioctl(_rcs_fd, TIOCSSWAP, SER_SWAP_ENABLED); +# endif // TIOCSSWAP +# ifdef TIOCSSINGLEWIRE + + if (rv != OK) { + ioctl(_rcs_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); + } + +# else + UNUSED(rv); +# endif // TIOCSSINGLEWIRE +#endif // RC_SERIAL_SWAP_USING_SINGLEWIRE +} + void RCInput::Run() { if (should_exit()) { @@ -494,6 +513,7 @@ void RCInput::Run() _rc_scan_begin = cycle_timestamp; // Configure serial port for DSM dsm_config(_rcs_fd); + swap_rx_tx(); // flush serial buffer and any existing buffered data tcflush(_rcs_fd, TCIOFLUSH); @@ -531,6 +551,7 @@ void RCInput::Run() _rc_scan_begin = cycle_timestamp; // Configure serial port for DSM dsm_config(_rcs_fd); + swap_rx_tx(); // flush serial buffer and any existing buffered data tcflush(_rcs_fd, TCIOFLUSH); @@ -582,6 +603,7 @@ void RCInput::Run() _rc_scan_begin = cycle_timestamp; // Configure serial port for DSM dsm_config(_rcs_fd); + swap_rx_tx(); // flush serial buffer and any existing buffered data tcflush(_rcs_fd, TCIOFLUSH); @@ -660,6 +682,7 @@ void RCInput::Run() _rc_scan_begin = cycle_timestamp; // Configure serial port for CRSF crsf_config(_rcs_fd); + swap_rx_tx(); // flush serial buffer and any existing buffered data tcflush(_rcs_fd, TCIOFLUSH); @@ -708,6 +731,7 @@ void RCInput::Run() _rc_scan_begin = cycle_timestamp; // Configure serial port for GHST ghst_config(_rcs_fd); + swap_rx_tx(); // flush serial buffer and any existing buffered data tcflush(_rcs_fd, TCIOFLUSH); diff --git a/src/drivers/rc_input/RCInput.hpp b/src/drivers/rc_input/RCInput.hpp index 718fb2647c6f..be781471c840 100644 --- a/src/drivers/rc_input/RCInput.hpp +++ b/src/drivers/rc_input/RCInput.hpp @@ -126,6 +126,7 @@ class RCInput : public ModuleBase, public ModuleParams, public px4::Sch void set_rc_scan_state(RC_SCAN _rc_scan_state); void rc_io_invert(bool invert); + void swap_rx_tx(void); input_rc_s _input_rc{}; hrt_abstime _rc_scan_begin{0}; diff --git a/src/drivers/tone_alarm/ToneAlarm.cpp b/src/drivers/tone_alarm/ToneAlarm.cpp index 5893f9018f7e..256f218b76f8 100644 --- a/src/drivers/tone_alarm/ToneAlarm.cpp +++ b/src/drivers/tone_alarm/ToneAlarm.cpp @@ -45,8 +45,6 @@ using namespace time_literals; ToneAlarm::ToneAlarm() : ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) { - // ensure ORB_ID(tune_control) is advertised with correct queue depth - orb_advertise_queue(ORB_ID(tune_control), nullptr, tune_control_s::ORB_QUEUE_LENGTH); } ToneAlarm::~ToneAlarm() diff --git a/src/drivers/uavcan/CMakeLists.txt b/src/drivers/uavcan/CMakeLists.txt index 37993c97ce51..39a9bb3ff2ff 100644 --- a/src/drivers/uavcan/CMakeLists.txt +++ b/src/drivers/uavcan/CMakeLists.txt @@ -75,6 +75,7 @@ add_definitions( -DUAVCAN_${UAVCAN_DRIVER_UPPER}_${OS_UPPER}=1 -DUAVCAN_${UAVCAN_DRIVER_UPPER}_NUM_IFACES=${config_uavcan_num_ifaces} -DUAVCAN_${UAVCAN_DRIVER_UPPER}_TIMER_NUMBER=${UAVCAN_TIMER} + -DUAVCAN_NUM_IFACES=${config_uavcan_num_ifaces} -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAVCAN_DRIVER=uavcan_${UAVCAN_DRIVER} -DUAVCAN_IMPLEMENT_PLACEMENT_NEW=1 @@ -167,6 +168,7 @@ px4_add_module( sensors/battery.cpp sensors/airspeed.cpp sensors/flow.cpp + sensors/gnss_relative.cpp sensors/gnss.cpp sensors/mag.cpp sensors/rangefinder.cpp diff --git a/src/drivers/uavcan/Kconfig b/src/drivers/uavcan/Kconfig index d2f064e8ba20..1d91acb6221e 100644 --- a/src/drivers/uavcan/Kconfig +++ b/src/drivers/uavcan/Kconfig @@ -57,6 +57,10 @@ if DRIVERS_UAVCAN bool "Subscribe to GPS: uavcan::equipment::gnss::Auxiliary | uavcan::equipment::gnss::Fix | uavcan::equipment::gnss::Fix2" default y + config UAVCAN_SENSOR_GNSS_RELATIVE + bool "Subscribe to GPS Relative: ardupilot::equipment::gnss::RelPosHeading" + default y + config UAVCAN_SENSOR_HYGROMETER bool "Subscribe to Hygrometer: dronecan::sensors::hygrometer::Hygrometer" default y diff --git a/src/drivers/uavcan/sensors/gnss.cpp b/src/drivers/uavcan/sensors/gnss.cpp index ddbc7907e641..49c033e43beb 100644 --- a/src/drivers/uavcan/sensors/gnss.cpp +++ b/src/drivers/uavcan/sensors/gnss.cpp @@ -155,7 +155,7 @@ UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure> 8; + spoofing_state = msg.ecef_position_velocity[0].position_xyz_mm[2] & 0xFF; } process_fixx(msg, fix_type, pos_cov, vel_cov, valid_covariances, valid_covariances, heading, heading_offset, - heading_accuracy); + heading_accuracy, noise_per_ms, jamming_indicator, jamming_state, spoofing_state); } template @@ -322,7 +335,9 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure const float (&pos_cov)[9], const float (&vel_cov)[9], const bool valid_pos_cov, const bool valid_vel_cov, const float heading, const float heading_offset, - const float heading_accuracy) + const float heading_accuracy, const int32_t noise_per_ms, + const int32_t jamming_indicator, const uint8_t jamming_state, + const uint8_t spoofing_state) { sensor_gps_s report{}; report.device_id = get_device_id(); @@ -452,6 +467,11 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure report.heading_offset = heading_offset; report.heading_accuracy = heading_accuracy; + report.noise_per_ms = noise_per_ms; + report.jamming_indicator = jamming_indicator; + report.jamming_state = jamming_state; + report.spoofing_state = spoofing_state; + publish(msg.getSrcNodeID().get(), &report); } diff --git a/src/drivers/uavcan/sensors/gnss.hpp b/src/drivers/uavcan/sensors/gnss.hpp index 5b65f57bb6fd..f2f28eb5bc86 100644 --- a/src/drivers/uavcan/sensors/gnss.hpp +++ b/src/drivers/uavcan/sensors/gnss.hpp @@ -89,7 +89,9 @@ class UavcanGnssBridge : public UavcanSensorBridgeBase const float (&pos_cov)[9], const float (&vel_cov)[9], const bool valid_pos_cov, const bool valid_vel_cov, const float heading, const float heading_offset, - const float heading_accuracy); + const float heading_accuracy, const int32_t noise_per_ms, + const int32_t jamming_indicator, const uint8_t jamming_state, + const uint8_t spoofing_state); void handleInjectDataTopic(); bool PublishRTCMStream(const uint8_t *data, size_t data_len); diff --git a/src/drivers/uavcan/sensors/gnss_relative.cpp b/src/drivers/uavcan/sensors/gnss_relative.cpp new file mode 100644 index 000000000000..16369391634b --- /dev/null +++ b/src/drivers/uavcan/sensors/gnss_relative.cpp @@ -0,0 +1,78 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include "gnss_relative.hpp" + +#include +#include + +const char *const UavcanGnssRelativeBridge::NAME = "gnss_relative"; + +UavcanGnssRelativeBridge::UavcanGnssRelativeBridge(uavcan::INode &node) : + UavcanSensorBridgeBase("uavcan_gnss_relative", ORB_ID(sensor_gnss_relative)), + _sub_rel_pos_heading(node) +{ +} + +int +UavcanGnssRelativeBridge::init() +{ + int res = _sub_rel_pos_heading.start(RelPosHeadingCbBinder(this, &UavcanGnssRelativeBridge::rel_pos_heading_sub_cb)); + + if (res < 0) { + DEVICE_LOG("failed to start uavcan sub: %d", res); + return res; + } + + return 0; +} + +void UavcanGnssRelativeBridge::rel_pos_heading_sub_cb(const + uavcan::ReceivedDataStructure &msg) +{ + sensor_gnss_relative_s sensor_gnss_relative{}; + + sensor_gnss_relative.timestamp_sample = uavcan::UtcTime(msg.timestamp).toUSec(); + + sensor_gnss_relative.heading_valid = msg.reported_heading_acc_available; + sensor_gnss_relative.heading = math::radians(msg.reported_heading_deg); + sensor_gnss_relative.heading_accuracy = math::radians(msg.reported_heading_acc_deg); + sensor_gnss_relative.position_length = msg.relative_distance_m; + sensor_gnss_relative.position[2] = msg.relative_down_pos_m; + + sensor_gnss_relative.device_id = get_device_id(); + + sensor_gnss_relative.timestamp = hrt_absolute_time(); + + publish(msg.getSrcNodeID().get(), &sensor_gnss_relative); +} diff --git a/src/modules/muorb/apps/fc_sensor.h b/src/drivers/uavcan/sensors/gnss_relative.hpp similarity index 62% rename from src/modules/muorb/apps/fc_sensor.h rename to src/drivers/uavcan/sensors/gnss_relative.hpp index 242a5a708baa..87c7e2273176 100644 --- a/src/modules/muorb/apps/fc_sensor.h +++ b/src/drivers/uavcan/sensors/gnss_relative.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2022 ModalAI, Inc. All rights reserved. + * 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 @@ -31,39 +31,35 @@ * ****************************************************************************/ -#ifndef FC_SENSOR_H -#define FC_SENSOR_H +#pragma once -#ifdef __cplusplus -extern "C" { -#endif +#include "sensor_bridge.hpp" #include -#include -typedef void (*fc_receive_cb)(const char *topic, - const uint8_t *data, - uint32_t length_in_bytes); -typedef void (*fc_advertise_cb)(const char *topic); -typedef void (*fc_add_subscription_cb)(const char *topic); -typedef void (*fc_remove_subscription_cb)(const char *topic); +#include -typedef struct { - fc_receive_cb rx_callback; - fc_advertise_cb ad_callback; - fc_add_subscription_cb sub_callback; - fc_remove_subscription_cb remove_sub_callback; -} fc_callbacks; +#include -int fc_sensor_initialize(bool enable_debug_messages, fc_callbacks *callbacks); -int fc_sensor_advertise(const char *topic); -int fc_sensor_subscribe(const char *topic); -int fc_sensor_unsubscribe(const char *topic); -int fc_sensor_send_data(const char *topic, - const uint8_t *data, - uint32_t length_in_bytes); -#ifdef __cplusplus -} -#endif +class UavcanGnssRelativeBridge : public UavcanSensorBridgeBase +{ +public: + static const char *const NAME; -#endif // FC_SENSOR_H + UavcanGnssRelativeBridge(uavcan::INode &node); + + const char *get_name() const override { return NAME; } + + int init() override; + +private: + + void rel_pos_heading_sub_cb(const uavcan::ReceivedDataStructure &msg); + + typedef uavcan::MethodBinder < UavcanGnssRelativeBridge *, + void (UavcanGnssRelativeBridge::*)(const uavcan::ReceivedDataStructure &) > + RelPosHeadingCbBinder; + + uavcan::Subscriber _sub_rel_pos_heading; + +}; diff --git a/src/drivers/uavcan/sensors/sensor_bridge.cpp b/src/drivers/uavcan/sensors/sensor_bridge.cpp index 3f1d424bd167..c219d90f3f04 100644 --- a/src/drivers/uavcan/sensors/sensor_bridge.cpp +++ b/src/drivers/uavcan/sensors/sensor_bridge.cpp @@ -60,6 +60,9 @@ #if defined(CONFIG_UAVCAN_SENSOR_GNSS) #include "gnss.hpp" #endif +#if defined(CONFIG_UAVCAN_SENSOR_GNSS_RELATIVE) +#include "gnss_relative.hpp" +#endif #if defined(CONFIG_UAVCAN_SENSOR_HYGROMETER) #include "hygrometer.hpp" #endif @@ -145,6 +148,17 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List= status_pub_interval) { + _last_can_status_pub = hrt_absolute_time(); + + for (int i = 0; i < _node.getDispatcher().getCanIOManager().getCanDriver().getNumIfaces(); i++) { + if (i > UAVCAN_NUM_IFACES) { + break; + } + + auto iface = _node.getDispatcher().getCanIOManager().getCanDriver().getIface(i); + + if (!iface) { + continue; + } + + auto iface_perf_cnt = _node.getDispatcher().getCanIOManager().getIfacePerfCounters(i); + can_interface_status_s status{ + .timestamp = hrt_absolute_time(), + .io_errors = iface_perf_cnt.errors, + .frames_tx = iface_perf_cnt.frames_tx, + .frames_rx = iface_perf_cnt.frames_rx, + .interface = static_cast(i), + }; + + if (_can_status_pub_handles[i] == nullptr) { + int instance{0}; + _can_status_pub_handles[i] = orb_advertise_multi(ORB_ID(can_interface_status), nullptr, &instance); + } + + (void)orb_publish(ORB_ID(can_interface_status), _can_status_pub_handles[i], &status); + } + } + // check for parameter updates if (_parameter_update_sub.updated()) { // clear update diff --git a/src/drivers/uavcan/uavcan_main.hpp b/src/drivers/uavcan/uavcan_main.hpp index 54119259ef16..5afa04ef672e 100644 --- a/src/drivers/uavcan/uavcan_main.hpp +++ b/src/drivers/uavcan/uavcan_main.hpp @@ -96,6 +96,7 @@ #include #include #include +#include #include #include #include @@ -309,6 +310,10 @@ class UavcanNode : public px4::ScheduledWorkItem, public ModuleParams uORB::Publication _param_response_pub{ORB_ID(uavcan_parameter_value)}; uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)}; + uORB::PublicationMulti _can_status_pub{ORB_ID(can_interface_status)}; + + hrt_abstime _last_can_status_pub{0}; + orb_advert_t _can_status_pub_handles[UAVCAN_NUM_IFACES] = {nullptr}; /* * The MAVLink parameter bridge needs to know the maximum parameter index diff --git a/src/drivers/uavcan/uavcan_params.c b/src/drivers/uavcan/uavcan_params.c index 21672f7c1edc..7a9b5ea51b3b 100644 --- a/src/drivers/uavcan/uavcan_params.c +++ b/src/drivers/uavcan/uavcan_params.c @@ -306,6 +306,18 @@ PARAM_DEFINE_INT32(UAVCAN_SUB_FLOW, 0); */ PARAM_DEFINE_INT32(UAVCAN_SUB_GPS, 1); +/** + * subscription GPS Relative + * + * Enable UAVCAN GPS Relative subscription. + * ardupilot::gnss::RelPosHeading + * + * @boolean + * @reboot_required true + * @group UAVCAN + */ +PARAM_DEFINE_INT32(UAVCAN_SUB_GPS_R, 1); + /** * subscription hygrometer * diff --git a/src/drivers/uavcannode/Kconfig b/src/drivers/uavcannode/Kconfig index 1e504ae201f6..d20c2baf0624 100644 --- a/src/drivers/uavcannode/Kconfig +++ b/src/drivers/uavcannode/Kconfig @@ -37,6 +37,10 @@ if DRIVERS_UAVCANNODE bool "Include hygrometer measurement" default n + config UAVCANNODE_INDICATED_AIR_SPEED + bool "Include Indicated Airspeed" + default n + config UAVCANNODE_LIGHTS_COMMAND bool "Include lights command" default n diff --git a/src/drivers/uavcannode/Publishers/GnssFix2.hpp b/src/drivers/uavcannode/Publishers/GnssFix2.hpp index 5cd977a1bfa3..4f3223ec489d 100644 --- a/src/drivers/uavcannode/Publishers/GnssFix2.hpp +++ b/src/drivers/uavcannode/Publishers/GnssFix2.hpp @@ -127,6 +127,11 @@ class GnssFix2 : ecefpositionvelocity.velocity_xyz[1] = NAN; ecefpositionvelocity.velocity_xyz[2] = NAN; + // Use ecef_position_velocity for now... There are no fields for these + ecefpositionvelocity.position_xyz_mm[0] = gps.noise_per_ms; + ecefpositionvelocity.position_xyz_mm[1] = gps.jamming_indicator; + ecefpositionvelocity.position_xyz_mm[2] = (gps.jamming_state << 8) | gps.spoofing_state; + // Use ecef_position_velocity for now... There is no heading field if (!std::isnan(gps.heading)) { ecefpositionvelocity.velocity_xyz[0] = gps.heading; @@ -138,10 +143,10 @@ class GnssFix2 : if (!std::isnan(gps.heading_accuracy)) { ecefpositionvelocity.velocity_xyz[2] = gps.heading_accuracy; } - - fix2.ecef_position_velocity.push_back(ecefpositionvelocity); } + fix2.ecef_position_velocity.push_back(ecefpositionvelocity); + uavcan::Publisher::broadcast(fix2); // ensure callback is registered diff --git a/src/drivers/uavcannode/Publishers/IndicatedAirspeed.hpp b/src/drivers/uavcannode/Publishers/IndicatedAirspeed.hpp new file mode 100644 index 000000000000..67982cf5f835 --- /dev/null +++ b/src/drivers/uavcannode/Publishers/IndicatedAirspeed.hpp @@ -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. + * + ****************************************************************************/ + +#pragma once + +#include "UavcanPublisherBase.hpp" + +#include +#include +#include + +namespace uavcannode +{ + +class IndicatedAirspeed : public UavcanPublisherBase, + public uORB::SubscriptionCallbackWorkItem, + private uavcan::Publisher < + uavcan::equipment::air_data::IndicatedAirspeed > +{ +public: + IndicatedAirspeed(px4::WorkItem *work_item, uavcan::INode &node) + : UavcanPublisherBase( + uavcan::equipment::air_data::IndicatedAirspeed::DefaultDataTypeID), + uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(airspeed)), + uavcan::Publisher( + node) + { + this->setPriority(uavcan::TransferPriority::OneLowerThanHighest); + } + + void PrintInfo() override + { + if (uORB::SubscriptionCallbackWorkItem::advertised()) { + printf( + "\t%s -> %s:%d\n", + uORB::SubscriptionCallbackWorkItem::get_topic()->o_name, + uavcan::equipment::air_data::IndicatedAirspeed::getDataTypeFullName(), + uavcan::equipment::air_data::IndicatedAirspeed::DefaultDataTypeID); + } + } + + void BroadcastAnyUpdates() override + { + + // airspeed -> uavcan::equipment::air_data::IndicatedAirspeed + + airspeed_s airspeed_m; + + if (uORB::SubscriptionCallbackWorkItem::update(&airspeed_m)) { + uavcan::equipment::air_data::IndicatedAirspeed indicated_as{}; + + indicated_as.indicated_airspeed = airspeed_m.indicated_airspeed_m_s; + uavcan::Publisher:: + broadcast(indicated_as); + + // ensure callback is registered + uORB::SubscriptionCallbackWorkItem::registerCallback(); + } + } +}; +} // namespace uavcannode diff --git a/src/drivers/uavcannode/UavcanNode.cpp b/src/drivers/uavcannode/UavcanNode.cpp index 40f1b8316ce0..61c96bd6b237 100644 --- a/src/drivers/uavcannode/UavcanNode.cpp +++ b/src/drivers/uavcannode/UavcanNode.cpp @@ -61,6 +61,10 @@ #include "Publishers/GnssAuxiliary.hpp" #endif // CONFIG_UAVCANNODE_GNSS_FIX +#if defined(CONFIG_UAVCANNODE_INDICATED_AIR_SPEED) +#include "Publishers/IndicatedAirspeed.hpp" +#endif // CONFIG_UAVCANNODE_INDICATED_AIR_SPEED + #if defined(CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH) #include "Publishers/MagneticFieldStrength2.hpp" #endif // CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH diff --git a/src/drivers/voxl2_io/CMakeLists.txt b/src/drivers/voxl2_io/CMakeLists.txt new file mode 100644 index 000000000000..57a5f4af21af --- /dev/null +++ b/src/drivers/voxl2_io/CMakeLists.txt @@ -0,0 +1,53 @@ +############################################################################ +# +# Copyright (c) 2015-2020 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_module( + MODULE drivers__voxl2_io + MAIN voxl2_io + SRCS + voxl2_io_crc16.c + voxl2_io_crc16.h + voxl2_io_serial.cpp + voxl2_io_serial.hpp + voxl2_io_packet.c + voxl2_io_packet.h + voxl2_io_packet_types.h + voxl2_io.cpp + voxl2_io.hpp + DEPENDS + rc + px4_work_queue + mixer_module + MODULE_CONFIG + module.yaml + ) diff --git a/src/drivers/voxl2_io/Kconfig b/src/drivers/voxl2_io/Kconfig new file mode 100644 index 000000000000..15964deeeb90 --- /dev/null +++ b/src/drivers/voxl2_io/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_VOXL2_IO + bool "voxl2_io" + default n + ---help--- + Enable support for voxl2_io \ No newline at end of file diff --git a/src/drivers/voxl2_io/module.yaml b/src/drivers/voxl2_io/module.yaml new file mode 100644 index 000000000000..02aa2122e3ea --- /dev/null +++ b/src/drivers/voxl2_io/module.yaml @@ -0,0 +1,13 @@ +module_name: VOXL2 IO Output +actuator_output: + config_parameters: + - param: 'VOXL2_IO_MIN' + label: 'PWM min value' + - param: 'VOXL2_IO_MAX' + label: 'PWM max value' + output_groups: + - param_prefix: VOXL2_IO + group_label: 'PWMs' + channel_label: 'PWM Channel' + num_channels: 4 + diff --git a/src/drivers/voxl2_io/voxl2_io.cpp b/src/drivers/voxl2_io/voxl2_io.cpp new file mode 100644 index 000000000000..352b47490618 --- /dev/null +++ b/src/drivers/voxl2_io/voxl2_io.cpp @@ -0,0 +1,1122 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "voxl2_io.hpp" + +#include + + +Voxl2IO::Voxl2IO() : + OutputModuleInterface(MODULE_NAME, px4::serial_port_to_wq(VOXL2_IO_DEFAULT_PORT)), + _mixing_output{"VOXL2_IO", VOXL2_IO_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}, + _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": module cycle")}, + _output_update_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": output update interval")) +{ + _mixing_output.setMaxNumOutputs(VOXL2_IO_OUTPUT_CHANNELS); + _uart_port = new Voxl2IoSerial(); + voxl2_io_packet_init(&_sbus_packet); +} + +Voxl2IO::~Voxl2IO() +{ + /* make sure servos are off */ + stop_all_pwms(); + + if (_uart_port) { + _uart_port->uart_close(); + _uart_port = nullptr; + } + + perf_free(_cycle_perf); + perf_free(_output_update_perf); +} + + +int Voxl2IO::init() +{ + int ret = PX4_OK; + + /* Open serial port in this thread */ + if (!_uart_port->is_open()) { + if (_uart_port->uart_open((const char *)_device, _parameters.baud_rate) == PX4_OK) { + /* Send PWM config to M0065... pwm_min and pwm_max */ + PX4_INFO("Opened UART connection to M0065 device on port %s", _device); + + } else { + PX4_ERR("Failed opening device"); + return PX4_ERROR; + } + } + + /* Verify connectivity and protocol version number */ + if (get_version_info() < 0) { + PX4_ERR("Failed to detect voxl2_io protocol version."); + return PX4_ERROR; + + } else { + if (_version_info.sw_version == VOXL2_IO_SW_PROTOCOL_VERSION + && _version_info.hw_version == VOXL2_IO_HW_PROTOCOL_VERSION) { + PX4_INFO("Detected M0065 protocol version. SW: %u HW: %u", _version_info.sw_version, _version_info.hw_version); + + } else { + PX4_ERR("Detected incorrect M0065 protocol version. SW: %u HW: %u", _version_info.sw_version, _version_info.hw_version); + return PX4_ERROR; + } + } + + /* Getting initial parameter values */ + ret = update_params(); + + if (ret != OK) { + return ret; + } + + /* Send PWM MIN/MAX to M0065 */ + update_pwm_config(); + + ScheduleOnInterval(_current_update_interval); + // ScheduleNow(); + return ret; +} + +int Voxl2IO::update_params() +{ + int ret = PX4_ERROR; + + updateParams(); + ret = load_params(&_parameters); + + if (ret == PX4_OK) { + _mixing_output.setAllDisarmedValues(VOXL2_IO_MIXER_DISARMED); + _mixing_output.setAllFailsafeValues(VOXL2_IO_MIXER_FAILSAFE); + _mixing_output.setAllMinValues(VOXL2_IO_MIXER_MIN); + _mixing_output.setAllMaxValues(VOXL2_IO_MIXER_MAX); + _pwm_fullscale = _parameters.pwm_max - _parameters.pwm_min; + } + + return ret; +} + +int Voxl2IO::load_params(voxl2_io_params_t *params) +{ + int ret = PX4_OK; + int32_t max = params->pwm_max; + int32_t min = params->pwm_min; + + // initialize out + for (int i = 0; i < VOXL2_IO_OUTPUT_CHANNELS; i++) { + params->function_map[i] = (int)OutputFunction::Disabled; + } + + /* UART config, PWM mode, and RC protocol*/ + param_get(param_find("VOXL2_IO_BAUD"), ¶ms->baud_rate); + param_get(param_find("RC_INPUT_PROTO"), ¶ms->param_rc_input_proto); + + /* PWM min, max, and failsafe values*/ + param_get(param_find("VOXL2_IO_MIN"), ¶ms->pwm_min); + param_get(param_find("VOXL2_IO_MAX"), ¶ms->pwm_max); + + /* PWM output functions */ + param_get(param_find("VOXL2_IO_FUNC1"), ¶ms->function_map[0]); + param_get(param_find("VOXL2_IO_FUNC2"), ¶ms->function_map[1]); + param_get(param_find("VOXL2_IO_FUNC3"), ¶ms->function_map[2]); + param_get(param_find("VOXL2_IO_FUNC4"), ¶ms->function_map[3]); + + /* Validate PWM min and max values */ + if (params->pwm_min > params->pwm_max) { + PX4_ERR("Invalid parameter VOXL2_IO_MIN. Please verify parameters."); + params->pwm_min = 0; + ret = PX4_ERROR; + } + + if (ret == PX4_OK && _uart_port->is_open() && (max != params->pwm_max || min != params->pwm_min)) { + PX4_INFO("Updating PWM params load_params"); + update_pwm_config(); + } + + return ret; +} + +void Voxl2IO::update_pwm_config() +{ + Command cmd; + uint8_t data[VOXL2_IO_BOARD_CONFIG_SIZE] = {static_cast((_parameters.pwm_min & 0xFF00) >> 8), static_cast(_parameters.pwm_min & 0xFF), + static_cast((_parameters.pwm_max & 0xFF00) >> 8), static_cast(_parameters.pwm_max & 0xFF) + }; + cmd.len = voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_CONFIG_BOARD_REQUEST, data, VOXL2_IO_BOARD_CONFIG_SIZE, cmd.buf, + sizeof(cmd.buf)); + + if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { + PX4_ERR("Failed to send config packet"); + + } else { + _bytes_sent += cmd.len; + _packets_sent++; + } +} + +int Voxl2IO::get_version_info() +{ + int res = 0 ; + int header = -1 ; + int info_packet = -1; + int read_retries = 3; + int read_succeeded = 0; + Command cmd; + + /* Request protocol version info from M0065 */ + cmd.len = voxl2_io_create_version_request_packet(0, cmd.buf, VOXL2_IO_VERSION_INFO_SIZE); + + if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { + PX4_ERR("Failed to send version info packet"); + + } else { + _bytes_sent += cmd.len; + _packets_sent++; + } + + /* Read response */ + px4_usleep(10000); + memset(&_read_buf, 0x00, READ_BUF_SIZE); + res = _uart_port->uart_read(_read_buf, READ_BUF_SIZE); + + while (read_retries) { + if (res) { + /* Get index of packer header */ + for (int index = 0; index < READ_BUF_SIZE; ++index) { + if (_read_buf[index] == VOXL2_IO_PACKET_TYPE_VERSION_RESPONSE) { + info_packet = index; + break; + } + + if (_read_buf[index] == VOXL2_IO_PACKET_HEADER) { + header = index; + } + } + + /* Try again in a bit if packet header not present yet... */ + if (header == -1 || info_packet == -1) { + if (_debug && header == -1) { PX4_ERR("Failed to find voxl2_io packet header, trying again... retries left: %i", read_retries); } + + if (_debug && info_packet == -1) { PX4_ERR("Failed to find version info packet header, trying again... retries left: %i", read_retries); } + + read_retries--; + flush_uart_rx(); + + if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { + PX4_ERR("Failed to send version info packet"); + + } else { + _bytes_sent += cmd.len; + _packets_sent++; + px4_usleep(2000); + } + + continue; + } + + /* Check if we got a valid packet...*/ + if (parse_response(&_read_buf[header], (uint8_t)VOXL2_IO_VERSION_INFO_SIZE)) { + if (_debug) { + PX4_ERR("Error parsing version info packet"); + PX4_INFO_RAW("[%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x]\n", + _read_buf[header + 0], _read_buf[header + 1], _read_buf[header + 2], _read_buf[header + 3], _read_buf[header + 4], + _read_buf[header + 5], + _read_buf[header + 6], _read_buf[header + 7], _read_buf[header + 8], _read_buf[header + 9], _read_buf[header + 10], + _read_buf[header + 11], + _read_buf[header + 12], _read_buf[header + 13], _read_buf[header + 14], _read_buf[header + 15], _read_buf[header + 16], + _read_buf[header + 17], + _read_buf[header + 18], _read_buf[header + 19], _read_buf[header + 20], _read_buf[header + 21], _read_buf[header + 22], + _read_buf[header + 23], + _read_buf[header + 24], _read_buf[header + 25], _read_buf[header + 26], _read_buf[header + 27], _read_buf[header + 28], + _read_buf[header + 29] + ); + } + + read_retries--; + flush_uart_rx(); + + if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { + PX4_ERR("Failed to send version info packet"); + + } else { + _bytes_sent += cmd.len; + _packets_sent++; + px4_usleep(2000); + } + + break; + + } else { + memcpy(&_version_info, &_read_buf[header], sizeof(VOXL2_IO_VERSION_INFO)); + read_succeeded = 1; + break; + } + + } else { + read_retries--; + + if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { + PX4_ERR("Failed to send version info packet"); + + } else { + _bytes_sent += cmd.len; + _packets_sent++; + px4_usleep(2000); + } + } + } + + if (! read_succeeded) { + return -EIO; + } + + return 0; +} + +bool Voxl2IO::updateOutputs(bool stop_motors, uint16_t outputs[input_rc_s::RC_INPUT_MAX_CHANNELS], + unsigned num_outputs, unsigned num_control_groups_updated) +{ + /* Stop Mixer while ESCs are being calibrated */ + if (_outputs_disabled) { + return 0; + } + + //in Run() we call _mixing_output.update(), which calls MixingOutput::limitAndUpdateOutputs which calls _interface.updateOutputs (this function) + //So, if Run() is blocked by a custom command, this function will not be called until Run is running again + int16_t _rate_req[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0}; + uint8_t _led_req[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0}; + int32_t _fb_idx = -1; + + if (num_outputs != VOXL2_IO_OUTPUT_CHANNELS) { + PX4_ERR("Num outputs != VOXL2_IO_OUTPUT_CHANNELS!"); + return false; + } + + for (int i = 0; i < VOXL2_IO_OUTPUT_CHANNELS; i++) { + // do not run any signal on disabled channels + if (!_mixing_output.isFunctionSet(i)) { + outputs[i] = 0; + } + + if (outputs[i]) { + _pwm_on = true; + } + + if (!_pwm_on || stop_motors) { + _rate_req[i] = 0; + + } else { + _rate_req[i] = outputs[i]; + } + + _pwm_values[i] = _rate_req[i]; + } + + Command cmd; + cmd.len = voxl2_io_create_pwm_packet4_fb(_rate_req[0], _rate_req[1], _rate_req[2], _rate_req[3], + _led_req[0], _led_req[1], _led_req[2], _led_req[3], + _fb_idx, cmd.buf, sizeof(cmd.buf)); + + if (_pwm_on && _debug) { + PX4_INFO("Mixer outputs"); + PX4_INFO("[%u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u]", + outputs[0], outputs[1], outputs[2], outputs[3], outputs[4], outputs[5], + outputs[6], outputs[7], outputs[8], outputs[9], outputs[10], outputs[11], + outputs[12], outputs[13], outputs[14], outputs[15], outputs[16], outputs[17] + ); + + // Debug messages for PWM 400Hz values sent to M0065 + uint16_t tics_1 = (_parameters.pwm_min + (_pwm_fullscale * ((double)outputs[0] / VOXL2_IO_MIXER_MAX))) * VOXL2_IO_TICS; + PX4_INFO("\tPWM CH1: %hu::%uus::%u tics", outputs[0], tics_1 / 24, tics_1); + uint16_t tics_2 = (_parameters.pwm_min + (_pwm_fullscale * ((double)outputs[1] / VOXL2_IO_MIXER_MAX))) * VOXL2_IO_TICS; + PX4_INFO("\tPWM CH2: %u::%uus::%u tics", outputs[1], tics_2 / 24, tics_2); + uint16_t tics_3 = (_parameters.pwm_min + (_pwm_fullscale * ((double)outputs[2] / VOXL2_IO_MIXER_MAX))) * VOXL2_IO_TICS; + PX4_INFO("\tPWM CH3: %u::%uus::%u tics", outputs[2], tics_3 / 24, tics_3); + uint16_t tics_4 = (_parameters.pwm_min + (_pwm_fullscale * ((double)outputs[3] / VOXL2_IO_MIXER_MAX))) * VOXL2_IO_TICS; + PX4_INFO("\tPWM CH4: %u::%uus::%u tics", outputs[3], tics_4 / 24, tics_4); + PX4_INFO(""); + } + + if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { + PX4_ERR("Failed to send packet"); + return false; + + } else { + _bytes_sent += cmd.len; + _packets_sent++; + } + + perf_count(_output_update_perf); + + return true; +} + +int Voxl2IO::flush_uart_rx() +{ + while (_uart_port->uart_read(_read_buf, sizeof(_read_buf)) > 0) {} + + return 0; +} + +static bool valid_port(int port) +{ + if (port == 2 || port == 6 || port == 7) { return true; } + + return false; +} + +int Voxl2IO::parse_response(uint8_t *buf, uint8_t len) +{ + for (int i = 0; i < len; i++) { + int16_t ret = voxl2_io_packet_process_char(buf[i], &_sbus_packet); + + if (ret > 0) { + uint8_t packet_type = voxl2_io_packet_get_type(&_sbus_packet); + uint8_t packet_size = voxl2_io_packet_get_size(&_sbus_packet); + + if (packet_type == VOXL2_IO_PACKET_TYPE_RC_DATA_RAW && packet_size == VOXL2_IO_SBUS_FRAME_SIZE) { + return 0; + + } else if (packet_type == VOXL2_IO_PACKET_TYPE_VERSION_RESPONSE && packet_size == sizeof(VOXL2_IO_VERSION_INFO)) { + return 0; + + } else { + return -1; + } + + } else { //parser error + switch (ret) { + case VOXL2_IO_ERROR_BAD_CHECKSUM: + if (_pwm_on && _debug) { PX4_WARN("BAD packet checksum"); } + + break; + + case VOXL2_IO_ERROR_BAD_LENGTH: + if (_pwm_on && _debug) { PX4_WARN("BAD packet length"); } + + break; + + case VOXL2_IO_ERROR_BAD_HEADER: + if (_pwm_on && _debug) { PX4_WARN("BAD packet header"); } + + break; + + case VOXL2_IO_NO_PACKET: + // if(_pwm_on) PX4_WARN("NO packet"); + break; + + default: + if (_pwm_on && _debug) { PX4_WARN("Unknown error: %i", ret); } + + break; + } + + return ret; + } + } + + return 0; +} + +void Voxl2IO::fill_rc_in(uint16_t raw_rc_count_local, + uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS], + hrt_abstime now, bool frame_drop, bool failsafe, + unsigned frame_drops, int rssi, input_rc_s &input_rc) +{ + // fill rc_in struct for publishing + input_rc.channel_count = raw_rc_count_local; + + if (input_rc.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) { + input_rc.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS; + } + + unsigned valid_chans = 0; + + for (unsigned i = 0; i < input_rc.channel_count; i++) { + input_rc.values[i] = raw_rc_values_local[i]; + + if (raw_rc_values_local[i] != UINT16_MAX) { + valid_chans++; + } + + // once filled, reset values back to default + _raw_rc_values[i] = UINT16_MAX; + } + + input_rc.timestamp = now; + input_rc.timestamp_last_signal = input_rc.timestamp; + input_rc.rc_ppm_frame_length = 0; + + /* fake rssi if no value was provided */ + if (rssi == -1) { + input_rc.rssi = 255; + + } else { + input_rc.rssi = rssi; + } + + if (valid_chans == 0) { + input_rc.rssi = 0; + } + + if (frame_drops) { + _sbus_frame_drops++; + } + + input_rc.rc_failsafe = failsafe; + input_rc.rc_lost = input_rc.rc_failsafe; + input_rc.rc_lost_frame_count = _sbus_frame_drops; + input_rc.rc_total_frame_count = ++_sbus_total_frames; +} + +int Voxl2IO::receive_sbus() +{ + int res = 0; + int header = -1; + int read_retries = 3; + int read_succeeded = 0; + voxl2_io_packet_init(&_sbus_packet); + + while (read_retries) { + memset(&_read_buf, 0x00, READ_BUF_SIZE); + res = _uart_port->uart_read(_read_buf, READ_BUF_SIZE); + + if (res) { + /* Get index of packer header */ + for (int index = 0; index < READ_BUF_SIZE; ++index) { + if (_read_buf[index] == VOXL2_IO_PACKET_HEADER) { + header = index; + break; + } + } + + /* Try again in a bit if packet header not present yet... */ + if (header == -1) { + if (_debug) { PX4_ERR("Failed to find SBUS packet header, trying again... retries left: %i", read_retries); } + + read_retries--; + continue; + } + + /* Check if we got a valid packet...*/ + if (parse_response(&_read_buf[header], (uint8_t)VOXL2_IO_SBUS_FRAME_SIZE)) { + if (_pwm_on && _debug) { + PX4_ERR("Error parsing QC RAW SBUS packet"); + PX4_INFO_RAW("[%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x]\n", + _read_buf[header + 0], _read_buf[header + 1], _read_buf[header + 2], _read_buf[header + 3], _read_buf[header + 4], + _read_buf[header + 5], + _read_buf[header + 6], _read_buf[header + 7], _read_buf[header + 8], _read_buf[header + 9], _read_buf[header + 10], + _read_buf[header + 11], + _read_buf[header + 12], _read_buf[header + 13], _read_buf[header + 14], _read_buf[header + 15], _read_buf[header + 16], + _read_buf[header + 17], + _read_buf[header + 18], _read_buf[header + 19], _read_buf[header + 20], _read_buf[header + 21], _read_buf[header + 22], + _read_buf[header + 23], + _read_buf[header + 24], _read_buf[header + 25], _read_buf[header + 26], _read_buf[header + 27], _read_buf[header + 28], + _read_buf[header + 29] + ); + } + + read_retries--; + break; + } + + input_rc_s input_rc; + uint16_t num_values; + bool sbus_failsafe = false; + bool sbus_frame_drop = false; + uint16_t max_channels = sizeof(_raw_rc_values) / sizeof(_raw_rc_values[0]); + hrt_abstime now = hrt_absolute_time(); + bool rc_updated = sbus_parse(now, &_read_buf[header + SBUS_PAYLOAD], SBUS_FRAME_SIZE, _raw_rc_values, &num_values, + &sbus_failsafe, &sbus_frame_drop, &_sbus_frame_drops, max_channels); + + if (rc_updated) { + if (_pwm_on && _debug) { + PX4_INFO("Decoded packet, header pos: %i", header); + PX4_INFO("[%u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u]", + _raw_rc_values[0], _raw_rc_values[1], _raw_rc_values[2], + _raw_rc_values[3], _raw_rc_values[4], _raw_rc_values[5], + _raw_rc_values[6], _raw_rc_values[7], _raw_rc_values[8], + _raw_rc_values[9], _raw_rc_values[10], _raw_rc_values[11], + _raw_rc_values[12], _raw_rc_values[13], _raw_rc_values[14], + _raw_rc_values[15], _raw_rc_values[16], _raw_rc_values[17] + ); + } + + input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SBUS; + fill_rc_in(num_values, _raw_rc_values, now, sbus_frame_drop, sbus_failsafe, _sbus_frame_drops, -1, input_rc); + + if (!input_rc.rc_lost && !input_rc.rc_failsafe) { + _rc_last_valid = input_rc.timestamp; + } + + input_rc.timestamp_last_signal = _rc_last_valid; + _rc_pub.publish(input_rc); + + _bytes_received += res; + _packets_received++; + read_succeeded = 1; + break; + + } else if (_pwm_on && _debug) { + PX4_ERR("Failed to decode SBUS packet, header pos: %i", header); + + if (sbus_frame_drop) { + PX4_WARN("SBUS frame dropped"); + } + + PX4_ERR("[%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x]", + _read_buf[header + 0], _read_buf[header + 1], _read_buf[header + 2], _read_buf[header + 3], _read_buf[header + 4], + _read_buf[header + 5], + _read_buf[header + 6], _read_buf[header + 7], _read_buf[header + 8], _read_buf[header + 9], _read_buf[header + 10], + _read_buf[header + 11], + _read_buf[header + 12], _read_buf[header + 13], _read_buf[header + 14], _read_buf[header + 15], _read_buf[header + 16], + _read_buf[header + 17], + _read_buf[header + 18], _read_buf[header + 19], _read_buf[header + 20], _read_buf[header + 21], _read_buf[header + 22], + _read_buf[header + 23], + _read_buf[header + 24], _read_buf[header + 25], _read_buf[header + 26], _read_buf[header + 27], _read_buf[header + 28], + _read_buf[header + 29] + ); + } + } + + read_retries--; + } + + if (! read_succeeded) { + _new_packet = false; + return -EIO; + } + + _new_packet = true; + return 0; +} + + +void Voxl2IO::Run() +{ + if (should_exit()) { + ScheduleClear(); + _mixing_output.unregister(); + + exit_and_cleanup(); + return; + } + + perf_begin(_cycle_perf); + + /* Handle RC */ + if (_rc_mode == RC_MODE::SCAN) { + if (receive_sbus() == PX4_OK) { + PX4_INFO("Found M0065 SBUS RC."); + _rc_mode = RC_MODE::SBUS; + } // Add more cases here for other protocols in the future.. + + } else if (_rc_mode == RC_MODE::SBUS) { + receive_sbus(); + } + + /* Only update outputs if we have new values from RC */ + if (_new_packet || _rc_mode == RC_MODE::EXTERNAL) { + _mixing_output.update(); //calls MixingOutput::limitAndUpdateOutputs which calls updateOutputs in this module + _new_packet = false; + } + + /* update PWM status if armed or if disarmed PWM values are set */ + _pwm_on = _mixing_output.armed().armed; + + /* check for parameter updates */ + if (!_pwm_on && _parameter_update_sub.updated()) { + /* clear update */ + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + /* update parameters from storage */ + update_params(); + } + + /* Don't process commands if pwm on */ + if (!_pwm_on) { + if (_current_cmd.valid()) { + PX4_INFO("sending %d commands with delay %dus", _current_cmd.repeats, _current_cmd.repeat_delay_us); + flush_uart_rx(); + + do { + PX4_INFO("CMDs left %d", _current_cmd.repeats); + + if (_uart_port->uart_write(_current_cmd.buf, _current_cmd.len) == _current_cmd.len) { + if (_current_cmd.repeats == 0) { + _current_cmd.clear(); + } + + + } else { + _bytes_sent += _current_cmd.len; + _packets_sent++; + + if (_current_cmd.retries == 0) { + _current_cmd.clear(); + PX4_ERR("Failed to send command, errno: %i", errno); + + } else { + _current_cmd.retries--; + PX4_ERR("Failed to send command, errno: %i", errno); + } + } + + px4_usleep(_current_cmd.repeat_delay_us); + } while (_current_cmd.repeats-- > 0); + + } else { + Command *new_cmd = _pending_cmd.load(); + + if (new_cmd) { + _current_cmd = *new_cmd; + _pending_cmd.store(nullptr); + } + } + } + + /* check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread) */ + _mixing_output.updateSubscriptions(true); + perf_end(_cycle_perf); +} + +int Voxl2IO::task_spawn(int argc, char *argv[]) +{ + Voxl2IO *instance = new Voxl2IO(); + + if (instance) { + int myoptind = 0; + int ch; + const char *myoptarg = nullptr; + + _object.store(instance); + _task_id = task_id_is_work_queue; + argv++; + + while ((ch = px4_getopt(argc - 1, argv, "vdep:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'v': + PX4_INFO("Verbose mode enabled"); + get_instance()->_debug = true; + break; + + case 'd': + PX4_INFO("M0065 PWM outputs disabled"); + get_instance()->_outputs_disabled = true; + break; + + case 'e': + PX4_INFO("M0065 using external RC"); + get_instance()->_rc_mode = RC_MODE::EXTERNAL; + break; + + case 'p': + if (valid_port(atoi(myoptarg))) { + snprintf(get_instance()->_device, 2, "%s", myoptarg); + + } else { + PX4_ERR("Bad UART port number: %s (must be 2, 6, or 7).", myoptarg); + _object.store(nullptr); + _task_id = -1; + return PX4_ERROR; + } + + break; + + default: + print_usage("Unknown command, parsing flags"); + break; + } + } + + if (instance->init() == PX4_OK) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + + +bool Voxl2IO::stop_all_pwms() +{ + int16_t _rate_req[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0}; + int16_t _led_req[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0}; + uint8_t _fb_idx = 0; + + Command cmd; + cmd.len = voxl2_io_create_pwm_packet4_fb(_rate_req[0], _rate_req[1], _rate_req[2], _rate_req[3], + _led_req[0], _led_req[1], _led_req[2], _led_req[3], + _fb_idx, cmd.buf, sizeof(cmd.buf)); + + if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { + PX4_ERR("Failed to send packet"); + return false; + + } else { + _bytes_sent += cmd.len; + _packets_sent++; + } + + return true; +} + +int Voxl2IO::send_cmd_thread_safe(Command *cmd) +{ + cmd->id = _cmd_id++; + _pending_cmd.store(cmd); + + /* wait until main thread processed it */ + while (_pending_cmd.load()) { + px4_usleep(1000); + } + + return 0; +} + +int Voxl2IO::calibrate_escs() +{ + + /* Disable outputs so Mixer isn't being a PITA while we calibrate */ + _outputs_disabled = true; + + Command cmd; + int32_t fb_idx = -1; + uint8_t data[VOXL2_IO_ESC_CAL_SIZE] {0}; + cmd.len = voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_TUNE_CONFIG, data, VOXL2_IO_ESC_CAL_SIZE, cmd.buf, + sizeof(cmd.buf)); + + if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { + PX4_ERR("ESC Calibration failed: Failed to send PWM OFF packet"); + _outputs_disabled = false; + return -1; + } + + /* Give user 10 seconds to plug in PWM cable for ESCs */ + PX4_INFO("Disconnected and reconnect your ESCs! (Calibration will start in ~10 seconds)"); + hrt_abstime start_cal = hrt_absolute_time(); + + while (hrt_elapsed_time(&start_cal) < 10000000) { + continue; + } + + /* PWM MAX 3 seconds */ + PX4_INFO("Writing PWM MAX for 3 seconds!"); + int16_t max_pwm[4] {VOXL2_IO_MIXER_MAX, VOXL2_IO_MIXER_MAX, VOXL2_IO_MIXER_MAX, VOXL2_IO_MIXER_MAX}; + + if (_debug) { PX4_INFO("%i %i %i %i", max_pwm[0], max_pwm[1], max_pwm[2], max_pwm[3]); } + + int16_t led_cmd[4] {0, 0, 0, 0}; + cmd.len = voxl2_io_create_pwm_packet4_fb(max_pwm[0], max_pwm[1], max_pwm[2], max_pwm[3], + led_cmd[0], led_cmd[1], led_cmd[2], led_cmd[3], + fb_idx, cmd.buf, sizeof(cmd.buf)); + + if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { + PX4_ERR("ESC Calibration failed: Failed to send PWM MAX packet"); + _outputs_disabled = false; + return -1; + + } else { + cmd.clear(); + } + + hrt_abstime start_pwm_max = hrt_absolute_time(); + + while (hrt_elapsed_time(&start_pwm_max) < 3000000) { + continue; + } + + /* PWM MIN 4 seconds */ + PX4_INFO("Writing PWM MIN for 4 seconds!"); + int16_t min_pwm[4] {VOXL2_IO_MIXER_MIN, VOXL2_IO_MIXER_MIN, VOXL2_IO_MIXER_MIN, VOXL2_IO_MIXER_MIN}; + + if (_debug) { PX4_INFO("%i %i %i %i", min_pwm[0], min_pwm[1], min_pwm[2], min_pwm[3]); } + + cmd.len = voxl2_io_create_pwm_packet4_fb(min_pwm[0], min_pwm[1], min_pwm[2], min_pwm[3], + led_cmd[0], led_cmd[1], led_cmd[2], led_cmd[3], + fb_idx, cmd.buf, sizeof(cmd.buf)); + + if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { + PX4_ERR("ESC Calibration failed: Failed to send PWM MIN packet"); + _outputs_disabled = false; + return -1; + } + + hrt_abstime start_pwm_min = hrt_absolute_time(); + + while (hrt_elapsed_time(&start_pwm_min) < 4000000) { + continue; + } + + PX4_INFO("ESC Calibration complete"); + + _outputs_disabled = false; + return 0; +} + +int Voxl2IO::custom_command(int argc, char *argv[]) +{ + int myoptind = 0; + int ch; + const char *myoptarg = nullptr; + + Command cmd; + uint8_t output_channel = 0xF; + int16_t rate = 0; + + uint32_t repeat_count = 100; + uint32_t repeat_delay_us = 10000; + + const char *verb = argv[argc - 1]; + + if ((strcmp(verb, "pwm")) == 0 && argc < 3) { + return print_usage("pwm command: missing args"); + + } else if (argc < 1) { + return print_usage("unknown command: missing args"); + } + + PX4_INFO("Executing the following command: %s", verb); + + /* start the FMU if not running */ + if (!strcmp(verb, "start")) { + if (!is_running()) { + return Voxl2IO::task_spawn(argc, argv); + } + } + + if (!strcmp(verb, "status")) { + if (!is_running()) { + PX4_INFO("Not running"); + return -1; + } + + return get_instance()->print_status(); + } + + if (!is_running()) { + PX4_INFO("Not running"); + return -1; + } + + if (!strcmp(verb, "calibrate_escs")) { + if (get_instance()->_outputs_disabled) { + PX4_WARN("Can't calibrate ESCs while outputs are disabled."); + return -1; + } + + return get_instance()->calibrate_escs(); + } + + while ((ch = px4_getopt(argc, argv, "c:n:t:r:p:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'c': + output_channel = atoi(myoptarg); + + if (output_channel > VOXL2_IO_OUTPUT_CHANNELS - 1) { + char reason[50]; + sprintf(reason, "Bad channel value: %d. Must be 0-%d.", output_channel, VOXL2_IO_OUTPUT_CHANNELS - 1); + print_usage(reason); + return 0; + } + + break; + + case 'n': + repeat_count = atoi(myoptarg); + + if (repeat_count < 1) { + print_usage("bad repeat_count"); + return 0; + } + + break; + + case 't': + repeat_delay_us = atoi(myoptarg); + + if (repeat_delay_us < 1) { + print_usage("bad repeat delay"); + return 0; + } + + break; + + case 'r': + rate = atoi(myoptarg); + break; + + case 'p': + if (valid_port(atoi(myoptarg))) { + snprintf(get_instance()->_device, 2, "%s", myoptarg); + + } else { + PX4_ERR("Bad UART port number: %s (must be 2, 6, or 7).", myoptarg); + return 0; + } + + break; + + default: + print_usage("Unknown command, parsing flags"); + return 0; + } + } + + if (!strcmp(verb, "pwm")) { + PX4_INFO("Output channel: %i", output_channel); + PX4_INFO("Repeat count: %i", repeat_count); + PX4_INFO("Repeat delay (us): %i", repeat_delay_us); + PX4_INFO("Rate: %i", rate); + + if (output_channel < VOXL2_IO_OUTPUT_CHANNELS) { + PX4_INFO("Request PWM for Output Channel: %i - PWM: %i", output_channel, rate); + int16_t rate_req[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0}; + uint8_t id_fb = 0; + + if (output_channel == + 0xFF) { //WARNING: this condition is not possible due to check 'if (esc_id < MODAL_IO_OUTPUT_CHANNELS)'. + rate_req[0] = rate; + rate_req[1] = rate; + rate_req[2] = rate; + rate_req[3] = rate; + + } else { + rate_req[output_channel] = rate; + id_fb = output_channel; + } + + cmd.len = voxl2_io_create_pwm_packet4_fb(rate_req[0], rate_req[1], rate_req[2], rate_req[3], + 0, 0, 0, 0, + id_fb, cmd.buf, sizeof(cmd.buf)); + + cmd.response = false; + cmd.repeats = repeat_count; + cmd.resp_delay_us = 1000; + cmd.repeat_delay_us = repeat_delay_us; + cmd.print_feedback = false; + + PX4_INFO("feedback id debug: %i", id_fb); + PX4_INFO("Sending UART M0065 power command %i", rate); + + if (get_instance()->_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { + PX4_ERR("Failed to send packet: stop PWMs"); + return -1; + + } else { + get_instance()->_bytes_sent += cmd.len; + get_instance()->_packets_sent++; + } + + } else { + print_usage("Invalid Output Channel, use 0-3"); + return 0; + } + } + + return print_usage("unknown custom command"); +} + +int Voxl2IO::print_status() +{ + PX4_INFO("Max update rate: %u Hz", 1000000 / _current_update_interval); + PX4_INFO("PWM Rate: 400 Hz"); // Only support 400 Hz for now + PX4_INFO("Outputs on: %s", _pwm_on ? "yes" : "no"); + PX4_INFO("FW version: v%u.%u", _version_info.sw_version, _version_info.hw_version); + PX4_INFO("RC Type: SBUS"); // Only support SBUS through M0065 for now + PX4_INFO("RC Connected: %s", hrt_absolute_time() - _rc_last_valid > 500000 ? "no" : "yes"); + PX4_INFO("RC Packets Received: %" PRIu16, _sbus_total_frames); + PX4_INFO("UART port: %s", _device); + PX4_INFO("UART open: %s", _uart_port->is_open() ? "yes" : "no"); + PX4_INFO("Packets sent: %" PRIu32, _packets_sent); + PX4_INFO(""); + PX4_INFO("Params: VOXL2_IO_BAUD: %" PRId32, _parameters.baud_rate); + PX4_INFO("Params: VOXL2_IO_FUNC1: %" PRId32, _parameters.function_map[0]); + PX4_INFO("Params: VOXL2_IO_FUNC2: %" PRId32, _parameters.function_map[1]); + PX4_INFO("Params: VOXL2_IO_FUNC3: %" PRId32, _parameters.function_map[2]); + PX4_INFO("Params: VOXL2_IO_FUNC4: %" PRId32, _parameters.function_map[3]); + + perf_print_counter(_cycle_perf); + perf_print_counter(_output_update_perf); + PX4_INFO(""); + _mixing_output.printStatus(); + return 0; +} + +int Voxl2IO::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This module is responsible for driving the output pins. For boards without a separate IO chip +(eg. Pixracer), it uses the main channels. On boards with an IO chip (eg. Pixhawk), it uses the AUX channels, and the +px4io driver is used for main ones. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("voxl2_io", "driver"); + PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the task"); + PRINT_MODULE_USAGE_PARAM_FLAG('v', "Verbose messages", false); + PRINT_MODULE_USAGE_PARAM_FLAG('d', "Disable PWM", false); + PRINT_MODULE_USAGE_PARAM_FLAG('e', "Disable RC", false); + PRINT_MODULE_USAGE_PARAM_INT('p', 2, 2, 7, "UART port", false); + PRINT_MODULE_USAGE_COMMAND_DESCR("calibrate_escs", "Calibrate ESCs min/max range"); + PRINT_MODULE_USAGE_COMMAND_DESCR("calibrate_escs", "Calibrate ESCs min/max range"); + PRINT_MODULE_USAGE_COMMAND_DESCR("pwm", "Open-Loop PWM test control request"); + PRINT_MODULE_USAGE_PARAM_INT('c', 0, 0, 3, "PWM OUTPUT Channel, 0-3", false); + PRINT_MODULE_USAGE_PARAM_INT('r', 0, 0, 800, "Duty Cycle value, 0 to 800", false); + PRINT_MODULE_USAGE_PARAM_INT('n', 100, 0, 1<<31, "Command repeat count, 0 to INT_MAX", false); + PRINT_MODULE_USAGE_PARAM_INT('t', 10000, 0, 1<<31, "Delay between repeated commands (microseconds), 0 to INT_MAX", false); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int voxl2_io_main(int argc, char *argv[]); + +int voxl2_io_main(int argc, char *argv[]) +{ + return Voxl2IO::main(argc, argv); +} diff --git a/src/drivers/voxl2_io/voxl2_io.hpp b/src/drivers/voxl2_io/voxl2_io.hpp new file mode 100644 index 000000000000..6ac46251bcc9 --- /dev/null +++ b/src/drivers/voxl2_io/voxl2_io.hpp @@ -0,0 +1,228 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "voxl2_io_serial.hpp" + +#include "voxl2_io_packet.h" +#include "voxl2_io_packet_types.h" + +using namespace time_literals; + +class Voxl2IO final : public ModuleBase, public OutputModuleInterface +{ +public: + Voxl2IO(); + ~Voxl2IO() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + /** @see ModuleBase::print_status() */ + int print_status() override; + + /** @see OutputModuleInterface */ + bool updateOutputs(bool stop_motors, uint16_t outputs[input_rc_s::RC_INPUT_MAX_CHANNELS], + unsigned num_outputs, unsigned num_control_groups_updated) override; + + virtual int init(); + + void update_pwm_config(); + int get_version_info(); + + struct Command { + uint16_t id = 0; + uint8_t len = 0; + uint16_t repeats = 0; + uint16_t repeat_delay_us = 2000; + uint8_t retries = 0; + bool response = false; + uint16_t resp_delay_us = 1000; + bool print_feedback = false; + + static const uint8_t BUF_SIZE = 128; + uint8_t buf[BUF_SIZE]; + + bool valid() const { return len > 0; } + void clear() { len = 0; } + }; + + int send_cmd_thread_safe(Command *cmd); + int receive_sbus(); + + void fill_rc_in(uint16_t raw_rc_count_local, + uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS], + hrt_abstime now, bool frame_drop, bool failsafe, + unsigned frame_drops, int rssi, input_rc_s &input_rc); +private: + void Run() override; + bool stop_all_pwms(); + + /* PWM Parameters */ + static constexpr uint32_t VOXL2_IO_CONFIG = 0; // Default to off + static constexpr uint32_t VOXL2_IO_BOARD_CONFIG_SIZE = 4; // PWM_MIN, PWM_MAX, 4 bytes + static constexpr uint32_t VOXL2_IO_ESC_CAL_SIZE = 1; + static constexpr uint32_t VOXL2_IO_DEFAULT_BAUD = 921600; + static constexpr uint16_t VOXL2_IO_OUTPUT_CHANNELS = 4; + static constexpr uint16_t VOXL2_IO_OUTPUT_DISABLED = 0; + + static constexpr uint32_t VOXL2_IO_WRITE_WAIT_US = 200; + static constexpr uint32_t VOXL2_IO_DISCONNECT_TIMEOUT_US = 500000; + + static constexpr uint16_t DISARMED_VALUE = 0; + + static constexpr uint16_t VOXL2_IO_MIXER_MIN = 0; + static constexpr uint16_t VOXL2_IO_MIXER_MAX = 800; + static constexpr uint16_t VOXL2_IO_MIXER_FAILSAFE = 0; + static constexpr uint16_t VOXL2_IO_MIXER_DISARMED = 0; + + static constexpr int32_t VOXL2_IO_DEFAULT_MIN = 1000; + static constexpr int32_t VOXL2_IO_DEFAULT_MAX = 2000; + static constexpr int32_t VOXL2_IO_DEFAULT_FAILSAFE = 900; + static constexpr int32_t VOXL2_IO_TICS = 24; // 24 tics per us on M0065 timer clks + + /* SBUS */ + static constexpr uint16_t VOXL2_IO_SBUS_FRAME_SIZE = 30; + static constexpr uint16_t SBUS_PAYLOAD = 3; + + /* M0065 version info */ + static constexpr uint16_t VOXL2_IO_VERSION_INFO_SIZE = 6; + static constexpr uint16_t VOXL2_IO_SW_PROTOCOL_VERSION = 1; + static constexpr uint16_t VOXL2_IO_HW_PROTOCOL_VERSION = 35; + VOXL2_IO_VERSION_INFO _version_info; + + /* Module update interval */ + static constexpr unsigned _current_update_interval{4000}; // 250 Hz + + typedef struct { + int32_t config{VOXL2_IO_CONFIG}; + int32_t baud_rate{VOXL2_IO_DEFAULT_BAUD}; + int32_t pwm_min{VOXL2_IO_DEFAULT_MIN}; + int32_t pwm_max{VOXL2_IO_DEFAULT_MAX}; + int32_t pwm_failsafe{VOXL2_IO_DEFAULT_FAILSAFE}; + int32_t param_rc_input_proto{0}; + int32_t param_rc_rssi_pwm_chan{0}; + int32_t function_map[VOXL2_IO_OUTPUT_CHANNELS] {0, 0, 0, 0}; + int32_t verbose_logging{0}; + } voxl2_io_params_t; + voxl2_io_params_t _parameters; + + typedef enum { + PWM_MODE_START = 0, + PWM_MODE_400, + PWM_MODE_END + } PWM_MODE; + + enum RC_MODE { + DISABLED = 0, + SBUS, + SPEKTRUM, + EXTERNAL, + SCAN + } _rc_mode{RC_MODE::SCAN}; + + /* QUP7, VOXL2 J19, /dev/slpi-uart-7*/ + char _device[10] {VOXL2_IO_DEFAULT_PORT}; + Voxl2IoSerial *_uart_port; + + /* Mixer output */ + MixingOutput _mixing_output; + + /* RC input */ + VOXL2_IOPacket _sbus_packet; + uint64_t _rc_last_valid; + uint16_t _raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS] {UINT16_MAX}; + unsigned _sbus_frame_drops{0}; + uint16_t _sbus_total_frames{0}; + bool _new_packet{false}; + + /* Publications */ + uORB::PublicationMulti _rc_pub{ORB_ID(input_rc)}; + + /* Subscriptions */ + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; + + bool _pwm_on{false}; + int32_t _pwm_fullscale{0}; + int16_t _pwm_values[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0}; + bool _outputs_disabled{false}; + + perf_counter_t _cycle_perf; + perf_counter_t _output_update_perf; + + bool _debug{false}; + uint16_t _cmd_id{0}; + Command _current_cmd; + px4::atomic _pending_cmd{nullptr}; + + static const uint8_t READ_BUF_SIZE = 128; + uint8_t _read_buf[READ_BUF_SIZE]; + uint32_t _bytes_sent{0}; + uint32_t _bytes_received{0}; + uint32_t _packets_sent{0}; + uint32_t _packets_received{0}; + + int parse_response(uint8_t *buf, uint8_t len); + int load_params(voxl2_io_params_t *params); + int update_params(); + int flush_uart_rx(); + int calibrate_escs(); +}; diff --git a/src/drivers/voxl2_io/voxl2_io_crc16.c b/src/drivers/voxl2_io/voxl2_io_crc16.c new file mode 100644 index 000000000000..229610b1e01b --- /dev/null +++ b/src/drivers/voxl2_io/voxl2_io_crc16.c @@ -0,0 +1,95 @@ +/**************************************************************************** + * Copyright (c) 2017 The Linux Foundation. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name The Linux Foundation nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY + * THIS LICENSE. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * In addition Supplemental Terms apply. See the SUPPLEMENTAL file. + * + ****************************************************************************/ + +#include +#include "voxl2_io_crc16.h" + +// Look-up table for fast CRC16 computations +const uint16_t voxl2_io_crc16_table[256] = { + 0x0000, 0xc0c1, 0xc181, 0x0140, 0xc301, 0x03c0, 0x0280, 0xc241, + 0xc601, 0x06c0, 0x0780, 0xc741, 0x0500, 0xc5c1, 0xc481, 0x0440, + 0xcc01, 0x0cc0, 0x0d80, 0xcd41, 0x0f00, 0xcfc1, 0xce81, 0x0e40, + 0x0a00, 0xcac1, 0xcb81, 0x0b40, 0xc901, 0x09c0, 0x0880, 0xc841, + 0xd801, 0x18c0, 0x1980, 0xd941, 0x1b00, 0xdbc1, 0xda81, 0x1a40, + 0x1e00, 0xdec1, 0xdf81, 0x1f40, 0xdd01, 0x1dc0, 0x1c80, 0xdc41, + 0x1400, 0xd4c1, 0xd581, 0x1540, 0xd701, 0x17c0, 0x1680, 0xd641, + 0xd201, 0x12c0, 0x1380, 0xd341, 0x1100, 0xd1c1, 0xd081, 0x1040, + 0xf001, 0x30c0, 0x3180, 0xf141, 0x3300, 0xf3c1, 0xf281, 0x3240, + 0x3600, 0xf6c1, 0xf781, 0x3740, 0xf501, 0x35c0, 0x3480, 0xf441, + 0x3c00, 0xfcc1, 0xfd81, 0x3d40, 0xff01, 0x3fc0, 0x3e80, 0xfe41, + 0xfa01, 0x3ac0, 0x3b80, 0xfb41, 0x3900, 0xf9c1, 0xf881, 0x3840, + 0x2800, 0xe8c1, 0xe981, 0x2940, 0xeb01, 0x2bc0, 0x2a80, 0xea41, + 0xee01, 0x2ec0, 0x2f80, 0xef41, 0x2d00, 0xedc1, 0xec81, 0x2c40, + 0xe401, 0x24c0, 0x2580, 0xe541, 0x2700, 0xe7c1, 0xe681, 0x2640, + 0x2200, 0xe2c1, 0xe381, 0x2340, 0xe101, 0x21c0, 0x2080, 0xe041, + 0xa001, 0x60c0, 0x6180, 0xa141, 0x6300, 0xa3c1, 0xa281, 0x6240, + 0x6600, 0xa6c1, 0xa781, 0x6740, 0xa501, 0x65c0, 0x6480, 0xa441, + 0x6c00, 0xacc1, 0xad81, 0x6d40, 0xaf01, 0x6fc0, 0x6e80, 0xae41, + 0xaa01, 0x6ac0, 0x6b80, 0xab41, 0x6900, 0xa9c1, 0xa881, 0x6840, + 0x7800, 0xb8c1, 0xb981, 0x7940, 0xbb01, 0x7bc0, 0x7a80, 0xba41, + 0xbe01, 0x7ec0, 0x7f80, 0xbf41, 0x7d00, 0xbdc1, 0xbc81, 0x7c40, + 0xb401, 0x74c0, 0x7580, 0xb541, 0x7700, 0xb7c1, 0xb681, 0x7640, + 0x7200, 0xb2c1, 0xb381, 0x7340, 0xb101, 0x71c0, 0x7080, 0xb041, + 0x5000, 0x90c1, 0x9181, 0x5140, 0x9301, 0x53c0, 0x5280, 0x9241, + 0x9601, 0x56c0, 0x5780, 0x9741, 0x5500, 0x95c1, 0x9481, 0x5440, + 0x9c01, 0x5cc0, 0x5d80, 0x9d41, 0x5f00, 0x9fc1, 0x9e81, 0x5e40, + 0x5a00, 0x9ac1, 0x9b81, 0x5b40, 0x9901, 0x59c0, 0x5880, 0x9841, + 0x8801, 0x48c0, 0x4980, 0x8941, 0x4b00, 0x8bc1, 0x8a81, 0x4a40, + 0x4e00, 0x8ec1, 0x8f81, 0x4f40, 0x8d01, 0x4dc0, 0x4c80, 0x8c41, + 0x4400, 0x84c1, 0x8581, 0x4540, 0x8701, 0x47c0, 0x4680, 0x8641, + 0x8201, 0x42c0, 0x4380, 0x8341, 0x4100, 0x81c1, 0x8081, 0x4040, +}; + +uint16_t voxl2_io_crc16_init() +{ + return 0xFFFF; +} + +uint16_t voxl2_io_crc16_byte(uint16_t prev_crc, const uint8_t new_byte) +{ + uint8_t lut = (prev_crc ^ new_byte) & 0xFF; + return (prev_crc >> 8) ^ voxl2_io_crc16_table[lut]; +} + +uint16_t voxl2_io_crc16(uint16_t prev_crc, uint8_t const *input_buffer, uint16_t input_length) +{ + uint16_t crc = prev_crc; + + while (input_length--) { + crc = voxl2_io_crc16_byte(crc, *input_buffer++); + } + + return crc; +} diff --git a/src/drivers/voxl2_io/voxl2_io_crc16.h b/src/drivers/voxl2_io/voxl2_io_crc16.h new file mode 100644 index 000000000000..c8e320edda20 --- /dev/null +++ b/src/drivers/voxl2_io/voxl2_io_crc16.h @@ -0,0 +1,65 @@ +/**************************************************************************** + * Copyright (c) 2017 The Linux Foundation. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name The Linux Foundation nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY + * THIS LICENSE. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * In addition Supplemental Terms apply. See the SUPPLEMENTAL file. + * + ****************************************************************************/ + +/* + * This file contains function prototypes for crc16 computations using polynomial 0x8005 + */ + +#ifndef CRC16_H_ +#define CRC16_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include + +// Returns the seed of crc output, which should be used when computing crc16 of a byte sequence +uint16_t voxl2_io_crc16_init(void); + +// Process one byte by providing crc16 from previous step and new byte to consume. +// Output is the new crc16 value +uint16_t voxl2_io_crc16_byte(uint16_t prev_crc, const uint8_t new_byte); + +// Process an array of bytes by providing crc16 from previous step (or seed), array of bytes and its length +// Output is the new crc16 value +uint16_t voxl2_io_crc16(uint16_t prev_crc, uint8_t const *input_buffer, uint16_t input_length); + +#ifdef __cplusplus +} +#endif + +#endif //CRC16_H_ diff --git a/src/drivers/voxl2_io/voxl2_io_packet.c b/src/drivers/voxl2_io/voxl2_io_packet.c new file mode 100644 index 000000000000..3de26acbfed7 --- /dev/null +++ b/src/drivers/voxl2_io/voxl2_io_packet.c @@ -0,0 +1,253 @@ +/**************************************************************************** + * Copyright (c) 2017 The Linux Foundation. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name The Linux Foundation nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY + * THIS LICENSE. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * In addition Supplemental Terms apply. See the SUPPLEMENTAL file. + * + ****************************************************************************/ + +#include "voxl2_io_packet.h" +#include "voxl2_io_packet_types.h" + +#include + +int32_t voxl2_io_create_version_request_packet(uint8_t id, uint8_t *out, uint16_t out_size) +{ + return voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_VERSION_REQUEST, &id, 1, out, out_size); +} + +int32_t voxl2_io_create_extended_version_request_packet(uint8_t id, uint8_t *out, uint16_t out_size) +{ + return voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_VERSION_EXT_REQUEST, &id, 1, out, out_size); +} + +int32_t voxl2_io_create_reset_packet(uint8_t id, uint8_t *out, uint16_t out_size) +{ + char payload[] = "RESET0"; + payload[5] += id; + + return voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_RESET_CMD, (uint8_t *)payload, 6 /*sizeof(payload)*/, out, out_size); +} + + +int32_t voxl2_io_create_sound_packet(uint8_t frequency, uint8_t duration, uint8_t power, uint8_t mask, uint8_t *out, + uint16_t out_size) +{ + uint8_t data[4] = {frequency, duration, power, mask}; + return voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_SOUND_CMD, (uint8_t *) & (data[0]), 4, out, out_size); +} + +int32_t voxl2_io_create_led_control_packet(uint8_t led_byte_1, uint8_t led_byte_2, uint8_t *out, uint16_t out_size) +{ + uint8_t data[2] = {led_byte_1, led_byte_2}; + return voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_LED_CMD, (uint8_t *) & (data[0]), 2, out, out_size); +} + +int32_t voxl2_io_create_set_id_packet(uint8_t id, uint8_t *out, uint16_t out_size) +{ + return voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_SET_ID_CMD, (uint8_t *)&id, 1, out, out_size); +} + +int32_t voxl2_io_create_pwm_packet4(int16_t pwm0, int16_t pwm1, int16_t pwm2, int16_t pwm3, + uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3, + uint8_t *out, uint16_t out_size) +{ + return voxl2_io_create_pwm_packet4_fb(pwm0, pwm1, pwm2, pwm3, led0, led1, led2, led3, -1, out, out_size); +} + +int32_t voxl2_io_create_pwm_packet4_fb(int16_t pwm0, int16_t pwm1, int16_t pwm2, int16_t pwm3, + uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3, + int32_t fb_id, uint8_t *out, uint16_t out_size) +{ + uint16_t data[5]; + uint16_t leds = 0; + + if (fb_id != -1) { fb_id = fb_id % 4; } + + //limit the pwm commands + + if (pwm0 > 800) { pwm0 = 800; } if (pwm0 < -800) { pwm0 = -800; } + + if (pwm1 > 800) { pwm1 = 800; } if (pwm1 < -800) { pwm1 = -800; } + + if (pwm2 > 800) { pwm2 = 800; } if (pwm2 < -800) { pwm2 = -800; } + + if (pwm3 > 800) { pwm3 = 800; } if (pwm3 < -800) { pwm3 = -800; } + + //least significant bit is used for feedback request + pwm0 &= ~(0x0001); pwm1 &= ~(0x0001); pwm2 &= ~(0x0001); pwm3 &= ~(0x0001); + + if (fb_id == 0) { pwm0 |= 0x0001; } if (fb_id == 1) { pwm1 |= 0x0001; } + + if (fb_id == 2) { pwm2 |= 0x0001; } if (fb_id == 3) { pwm3 |= 0x0001; } + + leds |= led0 & 0b00000111; + leds |= (led1 & 0b00000111) << 3; + leds |= ((uint16_t)(led2 & 0b00000111)) << 6; + leds |= ((uint16_t)(led3 & 0b00000111)) << 9; + + data[0] = pwm0; data[1] = pwm1; data[2] = pwm2; data[3] = pwm3; data[4] = leds; + return voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_PWM_CMD, (uint8_t *) & (data[0]), 10, out, out_size); +} + + +int32_t voxl2_io_create_rpm_packet4(int16_t rpm0, int16_t rpm1, int16_t rpm2, int16_t rpm3, + uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3, + uint8_t *out, uint16_t out_size) +{ + return voxl2_io_create_rpm_packet4_fb(rpm0, rpm1, rpm2, rpm3, led0, led1, led2, led3, -1, out, out_size); +} + +int32_t voxl2_io_create_rpm_packet4_fb(int16_t rpm0, int16_t rpm1, int16_t rpm2, int16_t rpm3, + uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3, + int32_t fb_id, uint8_t *out, uint16_t out_size) +{ + uint16_t data[5]; + uint16_t leds = 0; + + if (fb_id != -1) { fb_id = fb_id % 4; } + + //least significant bit is used for feedback request + rpm0 &= ~(0x0001); rpm1 &= ~(0x0001); rpm2 &= ~(0x0001); rpm3 &= ~(0x0001); + + if (fb_id == 0) { rpm0 |= 0x0001; } if (fb_id == 1) { rpm1 |= 0x0001; } + + if (fb_id == 2) { rpm2 |= 0x0001; } if (fb_id == 3) { rpm3 |= 0x0001; } + + leds |= led0 & 0b00000111; + leds |= (led1 & 0b00000111) << 3; + leds |= ((uint16_t)(led2 & 0b00000111)) << 6; + leds |= ((uint16_t)(led3 & 0b00000111)) << 9; + + data[0] = rpm0; data[1] = rpm1; data[2] = rpm2; data[3] = rpm3; data[4] = leds; + return voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_RPM_CMD, (uint8_t *) & (data[0]), 10, out, out_size); +} + +int32_t voxl2_io_create_packet(uint8_t type, uint8_t *data, uint16_t size, uint8_t *out, uint16_t out_size) +{ + uint16_t packet_size = size + 5; + + if (packet_size > 255) { return -1; } + + if (out_size < packet_size) { return -2; } + + out[0] = 0xAF; + out[1] = packet_size; + out[2] = type; + + memcpy(&(out[3]), data, size); + + uint16_t crc = voxl2_io_crc16_init(); + crc = voxl2_io_crc16(crc, &(out[1]), packet_size - 3); + + memcpy(&(out[packet_size - 2]), &crc, sizeof(uint16_t)); + + return packet_size; +} + + + + +//feed in a character and see if we got a complete packet +int16_t voxl2_io_packet_process_char(uint8_t c, VOXL2_IOPacket *packet) +{ + int16_t ret = VOXL2_IO_NO_PACKET; + + uint16_t chk_comp; + uint16_t chk_rcvd; + + if (packet->len_received >= (sizeof(packet->buffer) - 1)) { + packet->len_received = 0; + } + + //reset the packet and start parsing from beginning if length byte == header + //this can only happen if the packet is de-synced and last char of checksum + //ends up being equal to the header, in that case we can end up in endless loop + //unable to re-sync with the packet + if (packet->len_received == 1 && c == VOXL2_IO_PACKET_HEADER) { + packet->len_received = 0; + } + + switch (packet->len_received) { + case 0: //header + packet->bp = packet->buffer; //reset the pointer for storing data + voxl2_io_packet_checksum_reset(packet); //reset the checksum to starting value + + if (c != VOXL2_IO_PACKET_HEADER) { //check the packet header + packet->len_received = 0; + ret = VOXL2_IO_ERROR_BAD_HEADER; + break; + } + + packet->len_received++; + *(packet->bp)++ = c; + break; + + case 1: //length + packet->len_received++; + *(packet->bp)++ = c; + packet->len_expected = c; + + if (packet->len_expected >= (sizeof(packet->buffer) - 1)) { + packet->len_received = 0; + ret = VOXL2_IO_ERROR_BAD_LENGTH; + break; + } + + voxl2_io_packet_checksum_process_char(packet, c); + break; + + default: //rest of the packet + packet->len_received++; + *(packet->bp)++ = c; + + if (packet->len_received < (packet->len_expected - 1)) { //do not compute checksum of checksum (last 2 bytes) + voxl2_io_packet_checksum_process_char(packet, c); + } + + if (packet->len_received < packet->len_expected) { //waiting for more bytes + break; + } + + //grab the computed checksum and compare against the received value + chk_comp = voxl2_io_packet_checksum_get(packet); + + memcpy(&chk_rcvd, packet->bp - 2, sizeof(uint16_t)); + + if (chk_comp == chk_rcvd) { ret = packet->len_received; } + + else { ret = VOXL2_IO_ERROR_BAD_CHECKSUM; } + + packet->len_received = 0; + break; + } + + return ret; +} diff --git a/src/drivers/voxl2_io/voxl2_io_packet.h b/src/drivers/voxl2_io/voxl2_io_packet.h new file mode 100644 index 000000000000..7e6237017f92 --- /dev/null +++ b/src/drivers/voxl2_io/voxl2_io_packet.h @@ -0,0 +1,287 @@ +/**************************************************************************** + * Copyright (c) 2017 The Linux Foundation. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name The Linux Foundation nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY + * THIS LICENSE. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * In addition Supplemental Terms apply. See the SUPPLEMENTAL file. + * + ****************************************************************************/ + +/* + * This file contains function prototypes for Voxl2 IO UART interface + */ + +#ifndef VOXL2_IO_PACKET +#define VOXL2_IO_PACKET + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include "voxl2_io_crc16.h" + +// Define byte values that correspond to setting red, greed, and blue LEDs +#define VOXL2_IO_LED_RED_ON 1 +#define VOXL2_IO_LED_GREEN_ON 2 +#define VOXL2_IO_LED_BLUE_ON 4 + + +// Header of the packet. Each packet must start with this header +#define VOXL2_IO_PACKET_HEADER 0xAF + +enum { VOXL2_IO_ERROR_BAD_LENGTH = -3, + VOXL2_IO_ERROR_BAD_HEADER = -2, + VOXL2_IO_ERROR_BAD_CHECKSUM = -1, + VOXL2_IO_NO_PACKET = 0 + }; + +// Defines for the constatnt offsets of different parts of the packet +enum { VOXL2_IO_PACKET_POS_HEADER1 = 0, + VOXL2_IO_PACKET_POS_LENGTH, + VOXL2_IO_PACKET_POS_TYPE, + VOXL2_IO_PACKET_POS_DATA + }; + +// Definition of the structure that holds the state of the incoming data that is being recived (i.e. incomplete packets) +typedef struct { + uint8_t len_received; // Number of chars received so far + uint8_t len_expected; // Expected number of chars based on header + uint8_t *bp; // Pointer to the next write position in the buffer + uint16_t crc; // Accumulated CRC value so far + uint8_t buffer[64]; // Buffer to hold incoming data that is being parsed +} VOXL2_IOPacket; + + +// Definition of the response packet from ESC, containing the ESC version information +typedef struct { + uint8_t header; + uint8_t length; // Total length of the packet + uint8_t type; // This will be equal to ESC_PACKET_TYPE_VERSION_RESPONSE + + uint8_t id; // ID of the ESC that responded + uint16_t sw_version; // Software version of the ESC firmware + uint16_t hw_version; // HW version of the board + + uint32_t unique_id; // Unique ID of the ESC, if available + uint16_t crc; +} __attribute__((__packed__)) VOXL2_IO_VERSION_INFO; + +typedef struct { + uint8_t header; + uint8_t length; + uint8_t type; + uint8_t id; + uint16_t sw_version; + uint16_t hw_version; + uint8_t unique_id[12]; + char firmware_git_version[12]; + char bootloader_git_version[12]; + uint16_t bootloader_version; + uint16_t crc; +} __attribute__((__packed__)) VOXL2_IO_EXTENDED_VERSION_INFO; + +// Definition of the feedback response packet from ESC +typedef struct { + uint8_t header; + uint8_t length; // Total length of the packet + uint8_t type; // This will be equal to ESC_PACKET_TYPE_FB_RESPONSE + + uint8_t state; // bits 0:3 = state, bits 4:7 = ID + uint16_t rpm; // Current RPM of the motor + uint8_t cmd_counter; // Number of commands received by the ESC + uint8_t reserved0; + int8_t voltage; // Voltage = (-28)/34.0 + 9.0 = 8.176V. 0xE4 --> 228 (-28) + uint8_t reserved1; + + uint16_t crc; +} __attribute__((__packed__)) VOXL2_IO_FB_RESPONSE; + +// Definition of the feedback response packet from ESC +typedef struct { + uint8_t header; + uint8_t length; // Total length of the packet + uint8_t type; // This will be equal to ESC_PACKET_TYPE_FB_RESPONSE + uint8_t id_state; // bits 0:3 = state, bits 4:7 = ID + + uint16_t rpm; // Current RPM of the motor + uint8_t cmd_counter; // Number of commands received by the ESC + uint8_t power; // Applied power [0..100] + + uint16_t voltage; // Voltage measured by the ESC in mV + int16_t current; // Current measured by the ESC in 8mA resolution + int16_t temperature; // Temperature measured by the ESC in 0.01 degC resolution + + uint16_t crc; +} __attribute__((__packed__)) VOXL2_IO_FB_RESPONSE_V2; + + +// Definition of the feedback response packet from ESC, which contains battery voltage and total current +typedef struct { + uint8_t header; + uint8_t length; + uint8_t type; + uint8_t id; //ESC Id (could be used as system ID elsewhere) + uint16_t voltage; //Input voltage (Millivolts) + int16_t current; //Total Current (8mA resolution) + uint16_t crc; +} __attribute__((__packed__)) VOXL2_IO_FB_POWER_STATUS; + + +//------------------------------------------------------------------------- +//Below are functions for generating packets that would be outgoing to ESCs +//------------------------------------------------------------------------- + +// Create a generic packet by providing all required components +// Inputs are packet type, input data array and its size, output array and maximum size of output array +// Resulting packet will be placed in the output data array together with appropriate header and checksum +// Output value represents total length of the created packet (if positive), otherwise error code +int32_t voxl2_io_create_packet(uint8_t type, uint8_t *input_data, uint16_t input_size, uint8_t *out_data, + uint16_t out_data_size); + +// Create a packet for requesting version information from ESC with desired id +// If an ESC with this id is connected and receives this command, it will reply with it's version information +int32_t voxl2_io_create_version_request_packet(uint8_t id, uint8_t *out, uint16_t out_size); +int32_t voxl2_io_create_extended_version_request_packet(uint8_t id, uint8_t *out, uint16_t out_size); + +// Create a packet for requesting an ESC with desired id to reset +// When ESC with the particular id receives this command, and it's not spinning, ESC will reset +// This is useful for entering bootloader without removing power from the system +int32_t voxl2_io_create_reset_packet(uint8_t id, uint8_t *out, uint16_t out_size); + +// Create a packet for generating a tone packet (signals are applied to motor to generate sounds) +// Inputs are relative frequency (0-255), relative duration (0-255), power (0-255) and bit mask for which ESCs should play a tone +// Bit mask definition: if bit i is set to 1, then ESC with ID=i will generate the tone +// Note that tones can only be generated when motor is not spinning +int32_t voxl2_io_create_sound_packet(uint8_t frequency, uint8_t duration, uint8_t power, uint8_t mask, uint8_t *out, + uint16_t out_size); + +// Create a packet for standalone LED control +// Bit mask definition: +// led_byte_1 - bit0 = ESC0 Red, bit1 = ESC0, Green, bit2 = ESC0 Blue, bit3 = ESC1 Red, bit4 = ESC1 Green, +// bit5 = ESC1 Blue, bit6 = ESC2 Red, bit7 = ESC2 Green +// led_byte_2 - bit0 = ESC2 Blue, bit1 = ESC3 Red, bit2 = ESC3 Green, bit3 = ESC3 Blue, bits 4:7 = unused +// Note that control can only be sent when motor is not spinning +int32_t voxl2_io_create_led_control_packet(uint8_t led_byte_1, uint8_t led_byte_2, uint8_t *out, uint16_t out_size); + +// Create a packet for setting the ID of an ESC +// Return value is the length of generated packet (if positive), otherwise error code +// Note that all ESCs that will receive this command will be set to this ID +int32_t voxl2_io_create_set_id_packet(uint8_t id, uint8_t *out, uint16_t out_size); + +// Create a packet for sending open-loop command and LED command to 4 ESCs without requesting any feedback +// Return value is the length of generated packet (if positive), otherwise error code +int32_t voxl2_io_create_pwm_packet4(int16_t pwm0, int16_t pwm1, int16_t pwm2, int16_t pwm3, + uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3, + uint8_t *out, uint16_t out_size); + +// Create a packet for sending open-loop command and LED command to 4 ESCs, also request feedback from one ESC (with id=fb_id) +// Return value is the length of generated packet (if positive), otherwise error code +int32_t voxl2_io_create_pwm_packet4_fb(int16_t pwm0, int16_t pwm1, int16_t pwm2, int16_t pwm3, + uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3, + int32_t fb_id, uint8_t *out, uint16_t out_size); + +// Create a packet for sending closed-loop RPM command and LED command to 4 ESCs without requesting any feedback +// Return value is the length of generated packet (if positive), otherwise error code +int32_t voxl2_io_create_rpm_packet4(int16_t rpm0, int16_t rpm1, int16_t rpm2, int16_t rpm3, + uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3, + uint8_t *out, uint16_t out_size); + +// Create a packet for sending closed-loop RPM command and LED command to 4 ESCs, also request feedback from one ESC (with id=fb_id) +// Return value is the length of generated packet (if positive), otherwise error code +int32_t voxl2_io_create_rpm_packet4_fb(int16_t rpm0, int16_t rpm1, int16_t rpm2, int16_t rpm3, + uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3, + int32_t fb_id, uint8_t *out, uint16_t out_size); + + +//------------------------------------------------------------------------- +// Below are functions for processing incoming packets +//------------------------------------------------------------------------- + + +// Feed one char and see if we have accumulated a complete packet +int16_t voxl2_io_packet_process_char(uint8_t c, VOXL2_IOPacket *packet); + +// Get a pointer to the packet type from a pointer to VOXL2_IOPacket +static inline uint8_t voxl2_io_packet_get_type(VOXL2_IOPacket *packet) { return packet->buffer[VOXL2_IO_PACKET_POS_TYPE]; } + +// Get a pointer to the packet type from a uint8_t pointer that points to the raw packet data as it comes from UART port +static inline uint8_t voxl2_io_packet_raw_get_type(uint8_t *packet) { return packet[VOXL2_IO_PACKET_POS_TYPE]; } + +//get a pointer to the packet payload from a pointer to VOXL2_IOPacket +static inline uint8_t *voxl2_io_packet_get_data_ptr(VOXL2_IOPacket *packet) { return &(packet->buffer[VOXL2_IO_PACKET_POS_DATA]); } + +// Get a pointer to the packet payload from a uint8_t pointer that points to the raw packet data as it comes from UART port +static inline uint8_t *voxl2_io_packet_raw_get_data_ptr(uint8_t *packet) { return &(packet[VOXL2_IO_PACKET_POS_DATA]); } + +// Get the total size (length) in bytes of the packet +static inline uint8_t voxl2_io_packet_get_size(VOXL2_IOPacket *packet) { return packet->buffer[VOXL2_IO_PACKET_POS_LENGTH]; } + +// Get checksum of the packet from a pointer to VOXL2_IOPacket +static inline uint16_t voxl2_io_packet_checksum_get(VOXL2_IOPacket *packet) { return packet->crc; } + +// Calculate the checksum of a data array. Used for packet generation / processing +static inline uint16_t voxl2_io_packet_checksum(uint8_t *buf, uint16_t size) +{ + uint16_t crc = voxl2_io_crc16_init(); + return voxl2_io_crc16(crc, buf, size); +} + +// Reset the checksum of the incoming packet. Used internally for packet reception +static inline void voxl2_io_packet_checksum_reset(VOXL2_IOPacket *packet) { packet->crc = voxl2_io_crc16_init(); } + +// Process one character for checksum calculation while receiving a packet (used internally for packet reception) +static inline void voxl2_io_packet_checksum_process_char(VOXL2_IOPacket *packet, uint8_t c) +{ + packet->crc = voxl2_io_crc16_byte(packet->crc, c); +} + + +// Initialize an instance of an VOXL2_IOPacket. This should be called once before using an instance of VOXL2_IOPacket +static inline void voxl2_io_packet_init(VOXL2_IOPacket *packet) +{ + packet->len_received = 0; + packet->len_expected = 0; + packet->bp = 0; + + voxl2_io_packet_checksum_reset(packet); +} + +// Reset status of the packet that is being parsed. Effectively, this achieves the same thing as _packet_init +// so that _packet_init may be redundant +static inline void voxl2_io_packet_reset(VOXL2_IOPacket *packet) +{ + packet->len_received = 0; +} + +#endif //VOXL2_IO_PACKET + +#ifdef __cplusplus +} +#endif diff --git a/src/drivers/voxl2_io/voxl2_io_packet_types.h b/src/drivers/voxl2_io/voxl2_io_packet_types.h new file mode 100644 index 000000000000..150982eca4de --- /dev/null +++ b/src/drivers/voxl2_io/voxl2_io_packet_types.h @@ -0,0 +1,75 @@ +/**************************************************************************** + * Copyright (c) 2017 The Linux Foundation. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name The Linux Foundation nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY + * THIS LICENSE. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * In addition Supplemental Terms apply. See the SUPPLEMENTAL file. + * + ****************************************************************************/ + +/* + * This file contains the type values of all supported VOXL2_IO UART packets + */ + +#ifndef VOXL2_IO_PACKET_TYPES +#define VOXL2_IO_PACKET_TYPES + +#define VOXL2_IO_PACKET_TYPE_VERSION_REQUEST 0 +#define VOXL2_IO_PACKET_TYPE_PWM_CMD 1 +#define VOXL2_IO_PACKET_TYPE_RPM_CMD 2 +#define VOXL2_IO_PACKET_TYPE_SOUND_CMD 3 +#define VOXL2_IO_PACKET_TYPE_STEP_CMD 4 +#define VOXL2_IO_PACKET_TYPE_LED_CMD 5 +#define VOXL2_IO_PACKET_TYPE_RESET_CMD 10 +#define VOXL2_IO_PACKET_TYPE_SET_ID_CMD 11 +#define VOXL2_IO_PACKET_TYPE_SET_DIR_CMD 12 +#define VOXL2_IO_PACKET_TYPE_CONFIG_BOARD_REQUEST 20 +#define VOXL2_IO_PACKET_TYPE_CONFIG_USER_REQUEST 21 +#define VOXL2_IO_PACKET_TYPE_CONFIG_UART_REQUEST 22 +#define VOXL2_IO_PACKET_TYPE_CONFIG_TUNE_REQUEST 23 +#define VOXL2_IO_PACKET_TYPE_VERSION_EXT_REQUEST 24 + +#define VOXL2_IO_PACKET_TYPE_SET_FEEDBACK_MODE 50 //reserved for future use + +#define VOXL2_IO_PACKET_TYPE_EEPROM_WRITE_UNLOCK 70 +#define VOXL2_IO_PACKET_TYPE_EEPROM_READ_UNLOCK 71 +#define VOXL2_IO_PACKET_TYPE_EEPROM_WRITE 72 + +#define VOXL2_IO_PACKET_TYPE_VERSION_RESPONSE 109 +#define VOXL2_IO_PACKET_TYPE_PARAMS 110 +#define VOXL2_IO_PACKET_TYPE_BOARD_CONFIG 111 +#define VOXL2_IO_PACKET_TYPE_USER_CONFIG 112 +#define VOXL2_IO_PACKET_TYPE_UART_CONFIG 113 +#define VOXL2_IO_PACKET_TYPE_TUNE_CONFIG 114 +#define VOXL2_IO_PACKET_TYPE_FB_RESPONSE 128 +#define VOXL2_IO_PACKET_TYPE_VERSION_EXT_RESPONSE 131 +#define VOXL2_IO_PACKET_TYPE_FB_POWER_STATUS 132 +#define VOXL2_IO_PACKET_TYPE_RC_DATA_RAW 133 + +#endif diff --git a/src/drivers/voxl2_io/voxl2_io_params.c b/src/drivers/voxl2_io/voxl2_io_params.c new file mode 100644 index 000000000000..c718a4f62caf --- /dev/null +++ b/src/drivers/voxl2_io/voxl2_io_params.c @@ -0,0 +1,66 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * UART M0065 baud rate + * + * Default rate is 921600, which is used for communicating with M0065. + * + * @group VOXL2 IO + * @unit bit/s + */ +PARAM_DEFINE_INT32(VOXL2_IO_BAUD, 921600); + +/** + * M0065 PWM Min + * + * Minimum duration (microseconds) for M0065 PWM + * + * @min 0 + * @max 2000 + * @group VOXL2 IO + * @unit us + */ +PARAM_DEFINE_INT32(VOXL2_IO_MIN, 1000); + +/** + * M0065 PWM Max + * + * Maximum duration (microseconds) for M0065 PWM + * @min 0 + * @max 2000 + * @group VOXL2 IO + * @unit us + */ +PARAM_DEFINE_INT32(VOXL2_IO_MAX, 2000); + diff --git a/src/drivers/voxl2_io/voxl2_io_serial.cpp b/src/drivers/voxl2_io/voxl2_io_serial.cpp new file mode 100644 index 000000000000..cee0923ece06 --- /dev/null +++ b/src/drivers/voxl2_io/voxl2_io_serial.cpp @@ -0,0 +1,191 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "string.h" +#include "voxl2_io_serial.hpp" + +Voxl2IoSerial::Voxl2IoSerial() +{ +} + +Voxl2IoSerial::~Voxl2IoSerial() +{ + if (_uart_fd >= 0) { + uart_close(); + } +} + +int Voxl2IoSerial::uart_open(const char *dev, speed_t speed) +{ + if (_uart_fd >= 0) { + PX4_ERR("Port in use: %s (%i)", dev, errno); + return -1; + } + + /* Open UART */ +#ifdef __PX4_QURT + _uart_fd = qurt_uart_open(dev, speed); +#else + _uart_fd = open(dev, O_RDWR | O_NOCTTY | O_NONBLOCK); +#endif + + if (_uart_fd < 0) { + PX4_ERR("Error opening port: %s (%i)", dev, errno); + return -1; + } + +#ifndef __PX4_QURT + /* Back up the original UART configuration to restore it after exit */ + int termios_state; + + if ((termios_state = tcgetattr(_uart_fd, &_orig_cfg)) < 0) { + PX4_ERR("Error configuring port: tcgetattr %s: %d", dev, termios_state); + uart_close(); + return -1; + } + + /* Fill the struct for the new configuration */ + tcgetattr(_uart_fd, &_cfg); + + /* Disable output post-processing */ + _cfg.c_oflag &= ~OPOST; + + _cfg.c_cflag |= (CLOCAL | CREAD); /* ignore modem controls */ + _cfg.c_cflag &= ~CSIZE; + _cfg.c_cflag |= CS8; /* 8-bit characters */ + _cfg.c_cflag &= ~PARENB; /* no parity bit */ + _cfg.c_cflag &= ~CSTOPB; /* only need 1 stop bit */ + _cfg.c_cflag &= ~CRTSCTS; /* no hardware flowcontrol */ + + /* setup for non-canonical mode */ + _cfg.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON); + _cfg.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN); + + if (cfsetispeed(&_cfg, speed) < 0 || cfsetospeed(&_cfg, speed) < 0) { + PX4_ERR("Error configuring port: %s: %d (cfsetispeed, cfsetospeed)", dev, termios_state); + uart_close(); + return -1; + } + + if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &_cfg)) < 0) { + PX4_ERR("Error configuring port: %s (tcsetattr)", dev); + uart_close(); + return -1; + } + +#endif + + _speed = speed; + + return 0; +} + +int Voxl2IoSerial::uart_set_baud(speed_t speed) +{ +#ifndef __PX4_QURT + + if (_uart_fd < 0) { + return -1; + } + + if (cfsetispeed(&_cfg, speed) < 0) { + return -1; + } + + if (tcsetattr(_uart_fd, TCSANOW, &_cfg) < 0) { + return -1; + } + + _speed = speed; + + return 0; +#endif + + return -1; +} + +int Voxl2IoSerial::uart_close() +{ +#ifndef __PX4_QURT + + if (_uart_fd < 0) { + PX4_ERR("invalid state for closing"); + return -1; + } + + if (tcsetattr(_uart_fd, TCSANOW, &_orig_cfg)) { + PX4_ERR("failed restoring uart to original state"); + } + + if (close(_uart_fd)) { + PX4_ERR("error closing uart"); + } + +#endif + + _uart_fd = -1; + + return 0; +} + +int Voxl2IoSerial::uart_write(FAR void *buf, size_t len) +{ + if (_uart_fd < 0 || buf == NULL) { + PX4_ERR("invalid state for writing or buffer"); + return -1; + } + +#ifdef __PX4_QURT + return qurt_uart_write(_uart_fd, (const char *) buf, len); +#else + return write(_uart_fd, buf, len); +#endif +} + +int Voxl2IoSerial::uart_read(FAR void *buf, size_t len) +{ + if (_uart_fd < 0 || buf == NULL) { + PX4_ERR("invalid state for reading or buffer"); + return -1; + } + +#ifdef __PX4_QURT +#define ASYNC_UART_READ_WAIT_US 2000 + // The UART read on SLPI is via an asynchronous service so specify a timeout + // for the return. The driver will poll periodically until the read comes in + // so this may block for a while. However, it will timeout if no read comes in. + return qurt_uart_read(_uart_fd, (char *) buf, len, ASYNC_UART_READ_WAIT_US); +#else + return read(_uart_fd, buf, len); +#endif +} diff --git a/src/drivers/voxl2_io/voxl2_io_serial.hpp b/src/drivers/voxl2_io/voxl2_io_serial.hpp new file mode 100644 index 000000000000..638bdef28856 --- /dev/null +++ b/src/drivers/voxl2_io/voxl2_io_serial.hpp @@ -0,0 +1,69 @@ +/**************************************************************************** + * + * 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 + +#ifdef __PX4_QURT +#include +#define FAR +#endif + +class Voxl2IoSerial +{ +public: + Voxl2IoSerial(); + virtual ~Voxl2IoSerial(); + + int uart_open(const char *dev, speed_t speed); + int uart_set_baud(speed_t speed); + int uart_close(); + int uart_write(FAR void *buf, size_t len); + int uart_read(FAR void *buf, size_t len); + bool is_open() { return _uart_fd >= 0; }; + int uart_get_baud() {return _speed; } + +private: + int _uart_fd = -1; + +#if ! defined(__PX4_QURT) + struct termios _orig_cfg; + struct termios _cfg; +#endif + + int _speed = -1; +}; diff --git a/src/drivers/wind_sensor/Kconfig b/src/drivers/wind_sensor/Kconfig new file mode 100644 index 000000000000..128e91491d94 --- /dev/null +++ b/src/drivers/wind_sensor/Kconfig @@ -0,0 +1,10 @@ +menu "Wind Sensors" + menuconfig COMMON_WIND_SENSOR + bool "Common wind sensor's" + default n + select DRIVERS_WIND_SENSOR_FT_TECHNOLOGIES + ---help--- + Enable default set of wind sensor drivers + + rsource "*/Kconfig" +endmenu diff --git a/src/modules/differential_drive_control/CMakeLists.txt b/src/drivers/wind_sensor/ft_technologies/CMakeLists.txt similarity index 89% rename from src/modules/differential_drive_control/CMakeLists.txt rename to src/drivers/wind_sensor/ft_technologies/CMakeLists.txt index 3da82522fc86..5f9d9c2d63d7 100644 --- a/src/modules/differential_drive_control/CMakeLists.txt +++ b/src/drivers/wind_sensor/ft_technologies/CMakeLists.txt @@ -30,18 +30,17 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ - -add_subdirectory(DifferentialDriveKinematics) - px4_add_module( - MODULE modules__differential_drive_control - MAIN differential_drive_control + MODULE drivers__wind_sensor__ft_technologies + MAIN ft7_technologies + COMPILE_FLAGS + -Wno-cast-align # TODO: fix and enable SRCS - DifferentialDriveControl.cpp - DifferentialDriveControl.hpp + ft7_technologies.cpp + ft7_technologies.hpp + ft7_technologies_main.cpp DEPENDS - DifferentialDriveKinematics px4_work_queue MODULE_CONFIG module.yaml -) + ) diff --git a/src/drivers/wind_sensor/ft_technologies/Kconfig b/src/drivers/wind_sensor/ft_technologies/Kconfig new file mode 100644 index 000000000000..fa3f186952fc --- /dev/null +++ b/src/drivers/wind_sensor/ft_technologies/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_WIND_SENSOR_FT_TECHNOLOGIES + bool "ft7_technologies" + default n + ---help--- + Enable support for ft7_technologies diff --git a/src/drivers/wind_sensor/ft_technologies/ft7_technologies.cpp b/src/drivers/wind_sensor/ft_technologies/ft7_technologies.cpp new file mode 100644 index 000000000000..43f74c50ad66 --- /dev/null +++ b/src/drivers/wind_sensor/ft_technologies/ft7_technologies.cpp @@ -0,0 +1,359 @@ +/**************************************************************************** + * + * 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 "ft7_technologies.hpp" + +#include +#include +#include +#include + +Ft7Technologies::Ft7Technologies(const char *port) : + ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)), + _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), + _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err")) +{ + /* store port name */ + strncpy(_port, port, sizeof(_port) - 1); + + /* enforce null termination */ + _port[sizeof(_port) - 1] = '\0'; + + _sensor_airflow_pub.advertise(); +} + +Ft7Technologies::~Ft7Technologies() +{ + stop(); + + perf_free(_sample_perf); + perf_free(_comms_errors); +} + +int +Ft7Technologies::init() +{ + _interval = 1e6 / 10; // 10 Hz, The maximum query frequency is 10Hz + start(); + + return PX4_OK; +} + +int Ft7Technologies::measure() +{ + // Send the command to begin a measurement. + const char cmd[] = {'$', '/', '/', ',', 'W', 'V', '?', '*', '/', '/', '\r', '\n', '\0'}; + int ret = ::write(_fd, cmd, 12); + + perf_begin(_sample_perf); + + if (ret != 12) { + perf_count(_comms_errors); + return ret; + } + + _last_measure = hrt_absolute_time(); + + return PX4_OK; +} + +uint8_t Ft7Technologies::hex2int(char ch) +{ + if (ch >= '0' && ch <= '9') { + return ch - '0'; + } + + if (ch >= 'A' && ch <= 'F') { + return ch - 'A' + 10; + } + + if (ch >= 'a' && ch <= 'f') { + return ch - 'a' + 10; + } + + return -1; +} + +bool Ft7Technologies::checksum(char *buf, uint32_t checksum) +{ + + uint32_t checksum_verify = 0; + + for (int i = 0; i < _byte_counter; i++) { + + if (!(buf[i] == '$' || buf[i] == '*')) { + checksum_verify ^= buf[i]; + } + + if (buf[i] == '*') { + i = _byte_counter; + } + } + + return checksum_verify == checksum; +} + +int Ft7Technologies::collect() +{ + // PX4_INFO("collect"); + + /* clear buffer if last read was too long ago */ + // int64_t read_elapsed = hrt_elapsed_time(&_last_read); + + /* the buffer for read chars is buflen minus null termination */ + char readbuf[sizeof(_linebuf)]; + unsigned readlen = sizeof(readbuf) - 1; + + /* read from the sensor (uart buffer) */ + // const hrt_abstime timestamp_sample = hrt_absolute_time(); + + int ret = ::read(_fd, readbuf, readlen); + + // PX4_INFO("collect() ret: %d \n", ret); + + if (ret < 0) { + // PX4_INFO("read err: %d", ret); + return -EAGAIN; + + } else if (ret == 0) { + perf_count(_comms_errors); + return -EAGAIN; + } + + _last_read = hrt_absolute_time(); + + bool valid = false; + + //$,WVP=,,* + for (int i = 0; i < ret; i++) { + // _px4_windsensor.update(timestamp_sample, (double)ret, 13.0f, _status); + // received a full message + _readbuf[_byte_counter] = readbuf[i]; + _byte_counter += 1; + + + if (readbuf[i] == '\n') { + + _checksum = (uint32_t)atoi((char *)_raw_checksum); + _hex_checksum = 0; + + sensor_airflow_s sensor_airflow{}; + sensor_airflow.timestamp = hrt_absolute_time(); + sensor_airflow.speed = (float)atoi((char *)_raw_speed) / 10.0f; + sensor_airflow.direction = ((float)atoi((char *)_raw_angle) - 180.0f) * M_PI_F / 180.0f; + sensor_airflow.status = (uint8_t)atoi((char *)_raw_status); + + if (_checksum_counter == 2) { + _hex_checksum = hex2int(_raw_checksum[0]) << 4 | hex2int(_raw_checksum[1]); + + } else { + _hex_checksum = hex2int(_raw_checksum[0]); + } + + // checksum is verified + if (checksum(_readbuf, _hex_checksum)) { + + _sensor_airflow_pub.publish(sensor_airflow); + valid = true; + + } + + // reset counters + _msg_part_counter = 0; + _byte_counter = 0; + _msg_byte_counter = 0; + _checksum_counter = 0; + memset(readbuf, 0, sizeof(_linebuf)); + memset(_readbuf, 0, sizeof(_linebuf)); + memset(_raw_speed, 0, 5); + memset(_raw_angle, 0, 5); + memset(_raw_status, 0, 2); + memset(_raw_checksum, 0, 3); + + } + + else if (readbuf[i] == '$' || readbuf[i] == ',' || readbuf[i] == '=' || readbuf[i] == '*' || readbuf[i] == '\r') { + _msg_part_counter += 1; + _msg_byte_counter = 0; + + } else { + + if (readbuf[i] != '.') { + + if (_msg_part_counter == 3) { // speed measurement + _raw_speed[_msg_byte_counter] = readbuf[i]; + + } else if (_msg_part_counter == 4) { // angle measurement + _raw_angle[_msg_byte_counter] = readbuf[i]; + + } else if (_msg_part_counter == 5) { // status + _raw_status[_msg_byte_counter] = readbuf[i]; + + } else if (_msg_part_counter == 6) { // checksum + _checksum_counter += 1; + _raw_checksum[_msg_byte_counter] = readbuf[i]; + + } + + _msg_byte_counter += 1; + + } + + } + + } + + if (!valid) { + return -EAGAIN; + } + + perf_end(_sample_perf); + return PX4_OK; +} + +void Ft7Technologies::start() +{ + /* reset the report ring and state machine */ + _collect_phase = false; + + /* schedule a cycle to start things */ + ScheduleNow(); +} + +void Ft7Technologies::stop() +{ + ScheduleClear(); +} + +void Ft7Technologies::Run() +{ + + /* fds initialized? */ + if (_fd < 0) { + /* open fd */ + _fd = ::open(_port, O_RDWR | O_NOCTTY | O_NONBLOCK); + + if (_fd < 0) { + PX4_ERR("open failed (%i)", errno); + return; + } + + struct termios uart_config; + + int termios_state; + + /* fill the struct for the new configuration */ + tcgetattr(_fd, &uart_config); + + /* clear ONLCR flag (which appends a CR for every LF) */ + // uart_config.c_oflag &= ~ONLCR; + uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON); + + /* no parity, one stop bit */ + uart_config.c_oflag &= ~(OCRNL | ONLCR | ONLRET | ONOCR | OFILL | OPOST); + + // No line processing: + // echo off, echo newline off, canonical mode off, + // extended input processing off, signal chars off + uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG); + + // Turn off character processing + // clear current char size mask, no parity checking, + // no output processing, force 8 bit input + uart_config.c_cflag &= ~(CSIZE | PARENB); + + uart_config.c_cflag |= CS8; + + unsigned speed = B9600; + + /* 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(_fd, TCSANOW, &uart_config)) < 0) { + PX4_ERR("baud %d ATTR", termios_state); + } + + } + + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + int collect_ret = collect(); + + if (collect_ret == -EAGAIN) { + + if (hrt_elapsed_time(&_last_measure) > (_interval * 15) / 10) { + // resend the command again + // we waited to long to receive a response from the sensor + // so we are resending the command again + _collect_phase = false; + } + + ScheduleNow(); + return; + // we received a valid response from the sensor. we need to + // send another command through + + } else if (collect_ret == PX4_OK) { + + /* next phase is measurement */ + _collect_phase = false; + } + + } + + /* measurement phase */ + if (OK != measure()) { + PX4_INFO("measure error"); + } + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + ScheduleDelayed(_interval); +} + +void Ft7Technologies::print_info() +{ + PX4_INFO_RAW("%s: port: %s\n", MODULE_NAME, _port); + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); +} diff --git a/src/drivers/wind_sensor/ft_technologies/ft7_technologies.hpp b/src/drivers/wind_sensor/ft_technologies/ft7_technologies.hpp new file mode 100644 index 000000000000..816942797347 --- /dev/null +++ b/src/drivers/wind_sensor/ft_technologies/ft7_technologies.hpp @@ -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. + * + ****************************************************************************/ + +/** + * @file ft7_technolofies.hpp + * @author Henry Kotze + * + * Driver for the FT Technology Wind Sensor. FT742 + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace time_literals; + + +class Ft7Technologies : public px4::ScheduledWorkItem +{ +public: + Ft7Technologies(const char *port); + ~Ft7Technologies() override; + + int init(); + void print_info(); + +private: + + void start(); + void stop(); + void Run() override; + int measure(); + int collect(); + bool checksum(char *buf, uint32_t checksum); + uint8_t hex2int(char ch); + + uORB::Publication _sensor_airflow_pub{ORB_ID(sensor_airflow)}; + + char _port[20] {}; + char _readbuf[30] {}; + uint64_t _interval{100000}; + bool _collect_phase{false}; + int _fd{-1}; + char _linebuf[20] {}; + hrt_abstime _last_read{0}; + hrt_abstime _last_measure{0}; + + char _raw_angle[5]; + char _raw_speed[5]; + char _raw_status[2]; + char _raw_checksum[3]; + + uint16_t _checksum{0}; + uint32_t _hex_checksum{0}; + int _msg_part_counter{0}; + int _byte_counter{0}; + int _msg_byte_counter{0}; + uint32_t _checksum_counter{0}; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + +}; diff --git a/src/drivers/wind_sensor/ft_technologies/ft7_technologies_main.cpp b/src/drivers/wind_sensor/ft_technologies/ft7_technologies_main.cpp new file mode 100644 index 000000000000..85ec29b182ec --- /dev/null +++ b/src/drivers/wind_sensor/ft_technologies/ft7_technologies_main.cpp @@ -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. + * + ****************************************************************************/ + +#include "ft7_technologies.hpp" + +#include +#include + +namespace ft7_technologies_wind_sensor +{ + +Ft7Technologies *g_dev{nullptr}; + +static int start(const char *port) +{ + if (g_dev != nullptr) { + PX4_WARN("already started"); + return -1; + } + + if (port == nullptr) { + PX4_ERR("no device specified"); + return -1; + } + + /* create the driver */ + g_dev = new Ft7Technologies(port); + PX4_INFO("port name: %s", port); + + if (g_dev == nullptr) { + return -1; + } + + if (g_dev->init() != PX4_OK) { + delete g_dev; + g_dev = nullptr; + return -1; + } + + return 0; +} + +static int stop() +{ + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + + } else { + return -1; + } + + return 0; +} + +static int status() +{ + if (g_dev == nullptr) { + PX4_ERR("driver not running"); + return -1; + } + + g_dev->print_info(); + + return 0; +} + +static int usage() +{ + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description + +Serial bus driver for the FT Technologies Digital Wind Sensor FT742. This driver is required to operate alongside +a RS485 to UART signal transfer module. + +Most boards are configured to enable/start the driver on a specified UART using the SENS_FTX_CFG parameter. + +### Examples + +Attempt to start driver on a specified serial device. +$ ft_technologies_serial start -d /dev/ttyS1 +Stop driver +$ ft_technologies_serial stop +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("ft_technologies_serial", "driver"); + PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start driver"); + PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, nullptr, "Serial device", false); + PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop driver"); + return PX4_OK; +} + +} // namespace + +extern "C" __EXPORT int ft7_technologies_main(int argc, char *argv[]) +{ + const char *device_path = nullptr; + int ch; + int myoptind = 1; + const char *myoptarg = nullptr; + + while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'd': + device_path = myoptarg; + break; + + default: + ft7_technologies_wind_sensor::usage(); + return -1; + } + } + + if (myoptind >= argc) { + ft7_technologies_wind_sensor::usage(); + return -1; + } + + if (!strcmp(argv[myoptind], "start")) { + return ft7_technologies_wind_sensor::start(device_path); + + } else if (!strcmp(argv[myoptind], "stop")) { + return ft7_technologies_wind_sensor::stop(); + + } else if (!strcmp(argv[myoptind], "status")) { + return ft7_technologies_wind_sensor::status(); + } + + ft7_technologies_wind_sensor::usage(); + return -1; +} diff --git a/src/drivers/wind_sensor/ft_technologies/module.yaml b/src/drivers/wind_sensor/ft_technologies/module.yaml new file mode 100644 index 000000000000..4ff7dd05f74e --- /dev/null +++ b/src/drivers/wind_sensor/ft_technologies/module.yaml @@ -0,0 +1,6 @@ +module_name: FT Technologies Digital Wind Sensor (serial) +serial_config: + - command: ft7_technologies start -d ${SERIAL_DEV} + port_config_param: + name: SENS_FTX_CFG + group: Sensors diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 1d88b3b02256..4e20ce106ba8 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -67,6 +67,7 @@ add_subdirectory(pid_design EXCLUDE_FROM_ALL) add_subdirectory(rate_control EXCLUDE_FROM_ALL) add_subdirectory(rc EXCLUDE_FROM_ALL) add_subdirectory(ringbuffer EXCLUDE_FROM_ALL) +add_subdirectory(rtl EXCLUDE_FROM_ALL) add_subdirectory(sensor_calibration EXCLUDE_FROM_ALL) add_subdirectory(slew_rate EXCLUDE_FROM_ALL) add_subdirectory(systemlib EXCLUDE_FROM_ALL) diff --git a/src/lib/Kconfig b/src/lib/Kconfig new file mode 100644 index 000000000000..6353836988b2 --- /dev/null +++ b/src/lib/Kconfig @@ -0,0 +1 @@ +rsource "*/Kconfig" diff --git a/src/lib/adsb/AdsbConflict.h b/src/lib/adsb/AdsbConflict.h index 84ed38287dbb..b00d04f4dbd3 100644 --- a/src/lib/adsb/AdsbConflict.h +++ b/src/lib/adsb/AdsbConflict.h @@ -151,7 +151,7 @@ class AdsbConflict transponder_report_s tr{}; - orb_advert_t fake_traffic_report_publisher = orb_advertise_queue(ORB_ID(transponder_report), &tr, (unsigned int)20); + orb_advert_t fake_traffic_report_publisher = orb_advertise(ORB_ID(transponder_report), &tr); TRAFFIC_STATE _traffic_state_previous{TRAFFIC_STATE::NO_CONFLICT}; diff --git a/src/lib/battery/battery.cpp b/src/lib/battery/battery.cpp index 4408754d1aea..8c833af1e024 100644 --- a/src/lib/battery/battery.cpp +++ b/src/lib/battery/battery.cpp @@ -311,16 +311,23 @@ float Battery::computeRemainingTime(float current_a) if (_vehicle_status_sub.copy(&vehicle_status)) { _armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + _vehicle_status_is_fw = (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING); } } + _flight_phase_estimation_sub.update(); + if (!PX4_ISFINITE(_current_average_filter_a.getState()) || _current_average_filter_a.getState() < FLT_EPSILON) { _current_average_filter_a.reset(_params.bat_avrg_current); } if (_armed && PX4_ISFINITE(current_a)) { - // only update with positive numbers - _current_average_filter_a.update(fmaxf(current_a, 0.f)); + // For FW only update when we are in level flight + if (!_vehicle_status_is_fw || ((hrt_absolute_time() - _flight_phase_estimation_sub.get().timestamp) < 2_s + && _flight_phase_estimation_sub.get().flight_phase == flight_phase_estimation_s::FLIGHT_PHASE_LEVEL)) { + // only update with positive numbers + _current_average_filter_a.update(fmaxf(current_a, 0.f)); + } } // Remaining time estimation only possible with capacity diff --git a/src/lib/battery/battery.h b/src/lib/battery/battery.h index 66c923071e36..91edbe058053 100644 --- a/src/lib/battery/battery.h +++ b/src/lib/battery/battery.h @@ -56,6 +56,7 @@ #include #include #include +#include #include #include @@ -156,6 +157,7 @@ class Battery : public ModuleParams uORB::Subscription _vehicle_thrust_setpoint_0_sub{ORB_ID(vehicle_thrust_setpoint)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::SubscriptionData _flight_phase_estimation_sub{ORB_ID(flight_phase_estimation)}; uORB::PublicationMulti _battery_status_pub{ORB_ID(battery_status)}; bool _external_state_of_charge{false}; ///< inticates that the soc is injected and not updated by this library @@ -168,7 +170,8 @@ class Battery : public ModuleParams AlphaFilter _voltage_filter_v; float _current_a{-1}; AlphaFilter _current_filter_a; - AlphaFilter _current_average_filter_a; + AlphaFilter + _current_average_filter_a; ///< averaging filter for current. For FW, it is the current in level flight. AlphaFilter _throttle_filter; float _discharged_mah{0.f}; float _discharged_mah_loop{0.f}; @@ -178,5 +181,6 @@ class Battery : public ModuleParams uint8_t _warning{battery_status_s::BATTERY_WARNING_NONE}; hrt_abstime _last_timestamp{0}; bool _armed{false}; + bool _vehicle_status_is_fw{false}; hrt_abstime _last_unconnected_timestamp{0}; }; diff --git a/src/lib/battery/module.yaml b/src/lib/battery/module.yaml index c7750801b67b..44f379002870 100644 --- a/src/lib/battery/module.yaml +++ b/src/lib/battery/module.yaml @@ -1,4 +1,4 @@ -__max_num_config_instances: &max_num_config_instances 2 +__max_num_config_instances: &max_num_config_instances 3 module_name: battery @@ -21,7 +21,7 @@ parameters: reboot_required: true num_instances: *max_num_config_instances instance_start: 1 - default: [3.6, 3.6] + default: [3.6, 3.6, 3.6] BAT${i}_V_CHARGED: description: @@ -37,7 +37,7 @@ parameters: reboot_required: true num_instances: *max_num_config_instances instance_start: 1 - default: [4.05, 4.05] + default: [4.05, 4.05, 4.05] BAT${i}_V_LOAD_DROP: description: @@ -58,7 +58,7 @@ parameters: reboot_required: true num_instances: *max_num_config_instances instance_start: 1 - default: [0.1, 0.1] + default: [0.1, 0.1, 0.1] BAT${i}_R_INTERNAL: description: @@ -76,7 +76,7 @@ parameters: reboot_required: true num_instances: *max_num_config_instances instance_start: 1 - default: [0.005, 0.005] + default: [0.005, 0.005, 0.005] BAT${i}_N_CELLS: description: @@ -106,7 +106,7 @@ parameters: reboot_required: true num_instances: *max_num_config_instances instance_start: 1 - default: [0, 0] + default: [0, 0, 0] BAT${i}_CAPACITY: description: @@ -122,7 +122,7 @@ parameters: reboot_required: true num_instances: *max_num_config_instances instance_start: 1 - default: [-1.0, -1.0] + default: [-1.0, -1.0, -1.0] BAT${i}_SOURCE: description: @@ -142,4 +142,4 @@ parameters: reboot_required: true num_instances: *max_num_config_instances instance_start: 1 - default: [0, -1] + default: [0, -1, -1] diff --git a/src/lib/dataman_client/DatamanClient.cpp b/src/lib/dataman_client/DatamanClient.cpp index a6b37defa931..c2c0ec995fcd 100644 --- a/src/lib/dataman_client/DatamanClient.cpp +++ b/src/lib/dataman_client/DatamanClient.cpp @@ -39,6 +39,8 @@ DatamanClient::DatamanClient() { + _sync_perf = perf_alloc(PC_ELAPSED, "DatamanClient: sync"); + _dataman_request_pub.advertise(); _dataman_response_sub = orb_subscribe(ORB_ID(dataman_response)); @@ -74,6 +76,8 @@ DatamanClient::DatamanClient() DatamanClient::~DatamanClient() { + perf_free(_sync_perf); + if (_dataman_response_sub >= 0) { orb_unsubscribe(_dataman_response_sub); } @@ -85,6 +89,7 @@ bool DatamanClient::syncHandler(const dataman_request_s &request, dataman_respon bool response_received = false; int32_t ret = 0; hrt_abstime time_elapsed = hrt_elapsed_time(&start_time); + perf_begin(_sync_perf); _dataman_request_pub.publish(request); while (!response_received && (time_elapsed < timeout)) { @@ -132,6 +137,8 @@ bool DatamanClient::syncHandler(const dataman_request_s &request, dataman_respon time_elapsed = hrt_elapsed_time(&start_time); } + perf_end(_sync_perf); + if (!response_received && ret >= 0) { PX4_ERR("timeout after %" PRIu32 " ms!", static_cast(timeout / 1000)); } diff --git a/src/lib/dataman_client/DatamanClient.hpp b/src/lib/dataman_client/DatamanClient.hpp index 636b41d142af..9d1b71fd2cc4 100644 --- a/src/lib/dataman_client/DatamanClient.hpp +++ b/src/lib/dataman_client/DatamanClient.hpp @@ -62,7 +62,7 @@ class DatamanClient * * @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); + bool readSync(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length, hrt_abstime timeout = 5000_ms); /** * @brief Write data to the dataman synchronously. @@ -75,7 +75,7 @@ class DatamanClient * * @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); + bool writeSync(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length, hrt_abstime timeout = 5000_ms); /** * @brief Clears the data in the specified dataman item. @@ -85,7 +85,7 @@ class DatamanClient * * @return True if the operation was successful, false otherwise. */ - bool clearSync(dm_item_t item, hrt_abstime timeout = 1000_ms); + bool clearSync(dm_item_t item, hrt_abstime timeout = 5000_ms); /** * @brief Initiates an asynchronous request to read the data from dataman for a specific item and index. @@ -185,6 +185,8 @@ class DatamanClient uint8_t _client_id{0}; + perf_counter_t _sync_perf{nullptr}; + static constexpr uint8_t CLIENT_ID_NOT_SET{0}; }; @@ -246,7 +248,7 @@ class DatamanCache * * @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); + bool writeWait(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length, hrt_abstime timeout = 5000_ms); /** * @brief Updates the dataman cache by checking for responses from the DatamanClient and processing them. diff --git a/src/lib/events/enums.json b/src/lib/events/enums.json index fafe8c6e9bd4..8e86170951bf 100644 --- a/src/lib/events/enums.json +++ b/src/lib/events/enums.json @@ -364,7 +364,7 @@ }, "1": { "name": "manual_control_loss", - "description": "manual control loss" + "description": "Manual control loss" }, "2": { "name": "gcs_connection_loss", @@ -372,15 +372,19 @@ }, "3": { "name": "low_battery_level", - "description": "low battery level" + "description": "Low battery level" }, "4": { "name": "critical_battery_level", - "description": "critical battery level" + "description": "Critical battery level" }, "5": { "name": "emergency_battery_level", - "description": "emergency battery level" + "description": "Emergency battery level" + }, + "6": { + "name": "low_remaining_flight_time", + "description": "Remaining flight time low" } } }, @@ -398,19 +402,19 @@ }, "2": { "name": "fallback_posctrl", - "description": "fallback to position control" + "description": "Position mode" }, "3": { "name": "fallback_altctrl", - "description": "fallback to altitude control" + "description": "Altitude mode" }, "4": { "name": "fallback_stabilized", - "description": "fallback to stabilized" + "description": "Stabilized mode" }, "5": { "name": "hold", - "description": "hold" + "description": "Hold" }, "6": { "name": "rtl", @@ -418,11 +422,11 @@ }, "7": { "name": "land", - "description": "land" + "description": "Land" }, "8": { "name": "descend", - "description": "descend" + "description": "Descend" }, "9": { "name": "disarm", diff --git a/src/lib/events/events.cpp b/src/lib/events/events.cpp index c4d6867020c0..9bb215d3561f 100644 --- a/src/lib/events/events.cpp +++ b/src/lib/events/events.cpp @@ -61,7 +61,7 @@ void send(EventType &event) orb_publish(ORB_ID(event), orb_event_pub, &event); } else { - orb_event_pub = orb_advertise_queue(ORB_ID(event), &event, event_s::ORB_QUEUE_LENGTH); + orb_event_pub = orb_advertise(ORB_ID(event), &event); } pthread_mutex_unlock(&publish_event_mutex); diff --git a/src/lib/geo/test_geo.cpp b/src/lib/geo/test_geo.cpp index e93bdae0f6be..0cd3d835aaf4 100644 --- a/src/lib/geo/test_geo.cpp +++ b/src/lib/geo/test_geo.cpp @@ -42,7 +42,7 @@ class GeoTest : public ::testing::Test public: void SetUp() override { - proj.initReference(math::radians(473566094 / 1e7), math::radians(85190237 / 1e7), 0); + proj.initReference(473566094 / 1e7, 85190237 / 1e7, 0); } protected: @@ -73,7 +73,7 @@ TEST_F(GeoTest, reprojectProject) TEST_F(GeoTest, projectReproject) { - // GIVEN: x and y coordinates in the local cartesian frame + // GIVEN: lat and lon coordinates in the geographic coordinate system double lat = 47.356616973876953; double lon = 8.5190505981445313; float x; diff --git a/src/lib/mathlib/math/Utilities.hpp b/src/lib/mathlib/math/Utilities.hpp index 9b8f7047c422..890346b9733c 100644 --- a/src/lib/mathlib/math/Utilities.hpp +++ b/src/lib/mathlib/math/Utilities.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2024 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -51,17 +51,18 @@ static constexpr float sq(float var) { return var * var; } // rot312(1) - Second rotation is a RH rotation about the X axis (rad) // rot312(2) - Third rotation is a RH rotation about the Y axis (rad) // See http://www.atacolorado.com/eulersequences.doc -inline matrix::Dcmf taitBryan312ToRotMat(const matrix::Vector3f &rot312) +template +inline matrix::Dcm taitBryan312ToRotMat(const matrix::Vector3 &rot312) { // Calculate the frame2 to frame 1 rotation matrix from a 312 Tait-Bryan rotation sequence - const float c2 = cosf(rot312(2)); // third rotation is pitch - const float s2 = sinf(rot312(2)); - const float s1 = sinf(rot312(1)); // second rotation is roll - const float c1 = cosf(rot312(1)); - const float s0 = sinf(rot312(0)); // first rotation is yaw - const float c0 = cosf(rot312(0)); - - matrix::Dcmf R; + const T c2 = cosf(rot312(2)); // third rotation is pitch + const T s2 = sinf(rot312(2)); + const T s1 = sinf(rot312(1)); // second rotation is roll + const T c1 = cosf(rot312(1)); + const T s0 = sinf(rot312(0)); // first rotation is yaw + const T c0 = cosf(rot312(0)); + + matrix::Dcm R; R(0, 0) = c0 * c2 - s0 * s1 * s2; R(1, 1) = c0 * c1; R(2, 2) = c2 * c1; @@ -75,20 +76,21 @@ inline matrix::Dcmf taitBryan312ToRotMat(const matrix::Vector3f &rot312) return R; } -inline matrix::Dcmf quatToInverseRotMat(const matrix::Quatf &quat) +template +inline matrix::Dcm quatToInverseRotMat(const matrix::Quaternion &quat) { - const float q00 = quat(0) * quat(0); - const float q11 = quat(1) * quat(1); - const float q22 = quat(2) * quat(2); - const float q33 = quat(3) * quat(3); - const float q01 = quat(0) * quat(1); - const float q02 = quat(0) * quat(2); - const float q03 = quat(0) * quat(3); - const float q12 = quat(1) * quat(2); - const float q13 = quat(1) * quat(3); - const float q23 = quat(2) * quat(3); - - matrix::Dcmf dcm; + const T q00 = quat(0) * quat(0); + const T q11 = quat(1) * quat(1); + const T q22 = quat(2) * quat(2); + const T q33 = quat(3) * quat(3); + const T q01 = quat(0) * quat(1); + const T q02 = quat(0) * quat(2); + const T q03 = quat(0) * quat(3); + const T q12 = quat(1) * quat(2); + const T q13 = quat(1) * quat(3); + const T q23 = quat(2) * quat(3); + + matrix::Dcm dcm; dcm(0, 0) = q00 + q11 - q22 - q33; dcm(1, 1) = q00 - q11 + q22 - q33; dcm(2, 2) = q00 - q11 - q22 + q33; @@ -105,40 +107,46 @@ inline matrix::Dcmf quatToInverseRotMat(const matrix::Quatf &quat) // We should use a 3-2-1 Tait-Bryan (yaw-pitch-roll) rotation sequence // when there is more roll than pitch tilt and a 3-1-2 rotation sequence // when there is more pitch than roll tilt to avoid gimbal lock. -inline bool shouldUse321RotationSequence(const matrix::Dcmf &R) +template +inline bool shouldUse321RotationSequence(const matrix::Dcm &R) { return fabsf(R(2, 0)) < fabsf(R(2, 1)); } -inline float getEuler321Yaw(const matrix::Dcmf &R) +template +inline float getEuler321Yaw(const matrix::Dcm &R) { return atan2f(R(1, 0), R(0, 0)); } -inline float getEuler312Yaw(const matrix::Dcmf &R) +template +inline float getEuler312Yaw(const matrix::Dcm &R) { return atan2f(-R(0, 1), R(1, 1)); } -inline float getEuler321Yaw(const matrix::Quatf &q) +template +inline T getEuler321Yaw(const matrix::Quaternion &q) { // Values from yaw_input_321.c file produced by // https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw321.m - const float a = 2.f * (q(0) * q(3) + q(1) * q(2)); - const float b = sq(q(0)) + sq(q(1)) - sq(q(2)) - sq(q(3)); + const T a = static_cast(2.) * (q(0) * q(3) + q(1) * q(2)); + const T b = sq(q(0)) + sq(q(1)) - sq(q(2)) - sq(q(3)); return atan2f(a, b); } -inline float getEuler312Yaw(const matrix::Quatf &q) +template +inline T getEuler312Yaw(const matrix::Quaternion &q) { // Values from yaw_input_312.c file produced by // https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw312.m - const float a = 2.f * (q(0) * q(3) - q(1) * q(2)); - const float b = sq(q(0)) - sq(q(1)) + sq(q(2)) - sq(q(3)); + const T a = static_cast(2.) * (q(0) * q(3) - q(1) * q(2)); + const T b = sq(q(0)) - sq(q(1)) + sq(q(2)) - sq(q(3)); return atan2f(a, b); } -inline float getEulerYaw(const matrix::Dcmf &R) +template +inline T getEulerYaw(const matrix::Dcm &R) { if (shouldUse321RotationSequence(R)) { return getEuler321Yaw(R); @@ -148,23 +156,32 @@ inline float getEulerYaw(const matrix::Dcmf &R) } } -inline matrix::Dcmf updateEuler321YawInRotMat(float yaw, const matrix::Dcmf &rot_in) +template +inline T getEulerYaw(const matrix::Quaternion &q) { - matrix::Eulerf euler321(rot_in); + return getEulerYaw(matrix::Dcm(q)); +} + +template +inline matrix::Dcm updateEuler321YawInRotMat(T yaw, const matrix::Dcm &rot_in) +{ + matrix::Euler euler321(rot_in); euler321(2) = yaw; - return matrix::Dcmf(euler321); + return matrix::Dcm(euler321); } -inline matrix::Dcmf updateEuler312YawInRotMat(float yaw, const matrix::Dcmf &rot_in) +template +inline matrix::Dcm updateEuler312YawInRotMat(T yaw, const matrix::Dcm &rot_in) { - const matrix::Vector3f rotVec312(yaw, // yaw - asinf(rot_in(2, 1)), // roll - atan2f(-rot_in(2, 0), rot_in(2, 2))); // pitch + const matrix::Vector3 rotVec312(yaw, // yaw + asinf(rot_in(2, 1)), // roll + atan2f(-rot_in(2, 0), rot_in(2, 2))); // pitch return taitBryan312ToRotMat(rotVec312); } // Checks which euler rotation sequence to use and update yaw in rotation matrix -inline matrix::Dcmf updateYawInRotMat(float yaw, const matrix::Dcmf &rot_in) +template +inline matrix::Dcm updateYawInRotMat(T yaw, const matrix::Dcm &rot_in) { if (shouldUse321RotationSequence(rot_in)) { return updateEuler321YawInRotMat(yaw, rot_in); diff --git a/src/lib/matrix/matrix/AxisAngle.hpp b/src/lib/matrix/matrix/AxisAngle.hpp index 049e54d2771a..5cf351a3efb2 100644 --- a/src/lib/matrix/matrix/AxisAngle.hpp +++ b/src/lib/matrix/matrix/AxisAngle.hpp @@ -6,19 +6,16 @@ #pragma once -#include "math.hpp" +#include "Vector3.hpp" namespace matrix { -template -class Dcm; - template class Euler; -template -class AxisAngle; +template +class Quaternion; /** * AxisAngle class @@ -27,7 +24,7 @@ class AxisAngle; * described by this class. */ template -class AxisAngle : public Vector +class AxisAngle : public Vector3 { public: using Matrix31 = Matrix; @@ -38,7 +35,7 @@ class AxisAngle : public Vector * @param data_ array */ explicit AxisAngle(const Type data_[3]) : - Vector(data_) + Vector3(data_) { } @@ -53,7 +50,7 @@ class AxisAngle : public Vector * @param other Matrix31 to copy */ AxisAngle(const Matrix31 &other) : - Vector(other) + Vector3(other) { } @@ -133,7 +130,7 @@ class AxisAngle : public Vector { AxisAngle &v = *this; // make sure axis is a unit vector - Vector a = axis_; + Vector3 a = axis_; a = a.unit(); v(0) = a(0) * angle_; v(1) = a(1) * angle_; @@ -141,10 +138,10 @@ class AxisAngle : public Vector } - Vector axis() + Vector3 axis() { - if (Vector::norm() > 0) { - return Vector::unit(); + if (Vector3::norm() > 0) { + return Vector3::unit(); } else { return Vector3(1, 0, 0); @@ -153,7 +150,7 @@ class AxisAngle : public Vector Type angle() { - return Vector::norm(); + return Vector3::norm(); } }; diff --git a/src/lib/matrix/matrix/Dcm.hpp b/src/lib/matrix/matrix/Dcm.hpp index b64057209751..e05da5cbf3ee 100644 --- a/src/lib/matrix/matrix/Dcm.hpp +++ b/src/lib/matrix/matrix/Dcm.hpp @@ -15,19 +15,20 @@ #pragma once -#include "math.hpp" +#include "SquareMatrix.hpp" +#include "Vector3.hpp" namespace matrix { template -class Quaternion; +class AxisAngle; template class Euler; template -class AxisAngle; +class Quaternion; /** * Direction cosine matrix class @@ -39,8 +40,6 @@ template class Dcm : public SquareMatrix { public: - using Vector3 = Matrix; - /** * Standard constructor * @@ -158,14 +157,10 @@ class Dcm : public SquareMatrix dcm = Quaternion(aa); } - Vector vee() const // inverse to Vector.hat() operation + Vector3 vee() const // inverse to Vector.hat() operation { const Dcm &A(*this); - Vector v; - v(0) = -A(1, 2); - v(1) = A(0, 2); - v(2) = -A(0, 1); - return v; + return {-A(1, 2), A(0, 2), -A(0, 1)}; } void renormalize() diff --git a/src/lib/matrix/matrix/Dcm2.hpp b/src/lib/matrix/matrix/Dcm2.hpp index 97aee7a6224d..e74ea48b3e71 100644 --- a/src/lib/matrix/matrix/Dcm2.hpp +++ b/src/lib/matrix/matrix/Dcm2.hpp @@ -48,7 +48,8 @@ #pragma once -#include "math.hpp" +#include "SquareMatrix.hpp" +#include "Vector2.hpp" namespace matrix { @@ -113,10 +114,10 @@ class Dcm2 : public SquareMatrix void renormalize() { - /* renormalize rows */ + // renormalize rows for (size_t r = 0; r < 2; r++) { - matrix::Vector2 rvec(Matrix(this->Matrix::row(r)).transpose()); - this->Matrix::row(r) = rvec.normalized(); + Vector2 rvec(Matrix(this->row(r)).transpose()); + this->row(r) = rvec.normalized(); } } }; diff --git a/src/lib/matrix/matrix/Dual.hpp b/src/lib/matrix/matrix/Dual.hpp index fad15816234f..e7eafa033129 100644 --- a/src/lib/matrix/matrix/Dual.hpp +++ b/src/lib/matrix/matrix/Dual.hpp @@ -15,14 +15,12 @@ #include -#include "math.hpp" +#include "Scalar.hpp" +#include "Vector.hpp" namespace matrix { -template -class Vector; - template struct Dual { static constexpr size_t WIDTH = N; diff --git a/src/lib/matrix/matrix/Euler.hpp b/src/lib/matrix/matrix/Euler.hpp index d885b852154a..5a0bbdceda08 100644 --- a/src/lib/matrix/matrix/Euler.hpp +++ b/src/lib/matrix/matrix/Euler.hpp @@ -15,17 +15,9 @@ #pragma once -#include "math.hpp" - namespace matrix { -template -class Dcm; - -template -class Quaternion; - /** * Euler angles class * diff --git a/src/lib/matrix/matrix/LeastSquaresSolver.hpp b/src/lib/matrix/matrix/LeastSquaresSolver.hpp index 036b845ec4e3..80aed7cfee4e 100644 --- a/src/lib/matrix/matrix/LeastSquaresSolver.hpp +++ b/src/lib/matrix/matrix/LeastSquaresSolver.hpp @@ -14,7 +14,7 @@ #pragma once -#include "math.hpp" +#include "Vector.hpp" namespace matrix { diff --git a/src/lib/matrix/matrix/Matrix.hpp b/src/lib/matrix/matrix/Matrix.hpp index fbf5c501884e..14400683f0ef 100644 --- a/src/lib/matrix/matrix/Matrix.hpp +++ b/src/lib/matrix/matrix/Matrix.hpp @@ -12,20 +12,12 @@ #include #include -#include "math.hpp" +#include "helper_functions.hpp" +#include "Slice.hpp" namespace matrix { -template -class Vector; - -template -class Matrix; - -template -class Slice; - template class Matrix { @@ -75,6 +67,18 @@ class Matrix } } + template + Matrix(const ConstSlice &in_slice) + { + Matrix &self = *this; + + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + self(i, j) = in_slice(i, j); + } + } + } + /** * Accessors/ Assignment etc. */ @@ -158,6 +162,24 @@ class Matrix return res; } + // Using this function reduces the number of temporary variables needed to compute A * B.T + template + Matrix multiplyByTranspose(const Matrix &other) const + { + Matrix res; + const Matrix &self = *this; + + for (size_t i = 0; i < M; i++) { + for (size_t k = 0; k < P; k++) { + for (size_t j = 0; j < N; j++) { + res(i, k) += self(i, j) * other(k, j); + } + } + } + + return res; + } + // Element-wise multiplication Matrix emult(const Matrix &other) const { @@ -365,13 +387,52 @@ class Matrix } } - void print() const + void print(float eps = 1e-9) const { - // element: tab, point, 8 digits, 4 scientific notation chars; row: newline; string: \0 end - static const size_t n = 15 * N * M + M + 1; - char string[n]; - write_string(string, n); - printf("%s\n", string); + // print column numbering + if (N > 1) { + printf(" "); + + for (unsigned i = 0; i < N; i++) { + printf("|%2u ", i); + + } + + printf("\n"); + } + + const Matrix &self = *this; + + for (unsigned i = 0; i < M; i++) { + printf("%2u|", i); // print row numbering + + for (unsigned j = 0; j < N; j++) { + double d = static_cast(self(i, j)); + + // if symmetric don't print upper triangular elements + if ((M == N) && (j > i) && (i < N) && (j < M) + && (fabs(d - static_cast(self(j, i))) < (double)eps) + ) { + // print empty space + printf(" "); + + } else { + // avoid -0.0 for display + if (fabs(d - 0.0) < (double)eps) { + // print fixed width zero + printf(" 0 "); + + } else if ((fabs(d) < 1e-4) || (fabs(d) >= 10.0)) { + printf("% .1e ", d); + + } else { + printf("% 6.5f ", d); + } + } + } + + printf("\n"); + } } Matrix transpose() const @@ -395,18 +456,18 @@ class Matrix } template - const Slice slice(size_t x0, size_t y0) const + ConstSlice slice(size_t x0, size_t y0) const { - return Slice(x0, y0, this); + return {x0, y0, this}; } template Slice slice(size_t x0, size_t y0) { - return Slice(x0, y0, this); + return {x0, y0, this}; } - const Slice row(size_t i) const + ConstSlice row(size_t i) const { return slice<1, N>(i, 0); } @@ -416,7 +477,7 @@ class Matrix return slice<1, N>(i, 0); } - const Slice col(size_t j) const + ConstSlice col(size_t j) const { return slice(0, j); } diff --git a/src/lib/matrix/matrix/PseudoInverse.hpp b/src/lib/matrix/matrix/PseudoInverse.hpp index 37a084b351fa..70404aed7180 100644 --- a/src/lib/matrix/matrix/PseudoInverse.hpp +++ b/src/lib/matrix/matrix/PseudoInverse.hpp @@ -9,7 +9,8 @@ #pragma once -#include "math.hpp" +#include "SquareMatrix.hpp" +#include "Vector.hpp" namespace matrix { diff --git a/src/lib/matrix/matrix/Quaternion.hpp b/src/lib/matrix/matrix/Quaternion.hpp index 7155bdff1a6b..a2637606fd4c 100644 --- a/src/lib/matrix/matrix/Quaternion.hpp +++ b/src/lib/matrix/matrix/Quaternion.hpp @@ -28,21 +28,20 @@ #pragma once -#include "math.hpp" +#include // FLT_EPSILON + +#include "Vector3.hpp" +#include "Vector4.hpp" namespace matrix { -template -class Dcm; - template class Euler; template class AxisAngle; - /** * Quaternion class * diff --git a/src/lib/matrix/matrix/Scalar.hpp b/src/lib/matrix/matrix/Scalar.hpp index f34551fbd4f8..c40b32912bc3 100644 --- a/src/lib/matrix/matrix/Scalar.hpp +++ b/src/lib/matrix/matrix/Scalar.hpp @@ -8,7 +8,7 @@ #pragma once -#include "math.hpp" +#include "Matrix.hpp" namespace matrix { diff --git a/src/lib/matrix/matrix/Slice.hpp b/src/lib/matrix/matrix/Slice.hpp index da995a420b2c..57f7cfcb7002 100644 --- a/src/lib/matrix/matrix/Slice.hpp +++ b/src/lib/matrix/matrix/Slice.hpp @@ -8,8 +8,9 @@ #pragma once -#include "math.hpp" - +#include +#include +#include namespace matrix { @@ -20,14 +21,16 @@ class Matrix; template class Vector; -template -class Slice +template +class SliceT { public: - Slice(size_t x0, size_t y0, const Matrix *data) : + using Self = SliceT; + + SliceT(size_t x0, size_t y0, MatrixT *data) : _x0(x0), _y0(y0), - _data(const_cast*>(data)) + _data(data) { static_assert(P <= M, "Slice rows bigger than backing matrix"); static_assert(Q <= N, "Slice cols bigger than backing matrix"); @@ -35,7 +38,7 @@ class Slice assert(y0 + Q <= N); } - Slice(const Slice &other) = default; + SliceT(const Self &other) = default; const Type &operator()(size_t i, size_t j) const { @@ -46,7 +49,6 @@ class Slice } Type &operator()(size_t i, size_t j) - { assert(i < P); assert(j < Q); @@ -55,15 +57,29 @@ class Slice } // Separate function needed otherwise the default copy constructor matches before the deep copy implementation - Slice &operator=(const Slice &other) + Self &operator=(const Self &other) { return this->operator=(other); } template - Slice &operator=(const Slice &other) + Self &operator=(const SliceT, Type, P, Q, MM, NN> &other) + { + Self &self = *this; + + for (size_t i = 0; i < P; i++) { + for (size_t j = 0; j < Q; j++) { + self(i, j) = other(i, j); + } + } + + return self; + } + + template + SliceT &operator=(const SliceT, Type, P, Q, MM, NN> &other) { - Slice &self = *this; + SliceT &self = *this; for (size_t i = 0; i < P; i++) { for (size_t j = 0; j < Q; j++) { @@ -74,9 +90,9 @@ class Slice return self; } - Slice &operator=(const Matrix &other) + SliceT &operator=(const Matrix &other) { - Slice &self = *this; + SliceT &self = *this; for (size_t i = 0; i < P; i++) { for (size_t j = 0; j < Q; j++) { @@ -87,9 +103,9 @@ class Slice return self; } - Slice &operator=(const Type &other) + SliceT &operator=(const Type &other) { - Slice &self = *this; + SliceT &self = *this; for (size_t i = 0; i < P; i++) { for (size_t j = 0; j < Q; j++) { @@ -102,9 +118,9 @@ class Slice // allow assigning vectors to a slice that are in the axis template // make this a template function since it only exists for some instantiations - Slice &operator=(const Vector &other) + SliceT &operator=(const Vector &other) { - Slice &self = *this; + SliceT &self = *this; for (size_t j = 0; j < Q; j++) { self(0, j) = other(j); @@ -114,9 +130,9 @@ class Slice } template - Slice &operator+=(const Slice &other) + SliceT &operator+=(const SliceT &other) { - Slice &self = *this; + SliceT &self = *this; for (size_t i = 0; i < P; i++) { for (size_t j = 0; j < Q; j++) { @@ -127,9 +143,9 @@ class Slice return self; } - Slice &operator+=(const Matrix &other) + SliceT &operator+=(const Matrix &other) { - Slice &self = *this; + SliceT &self = *this; for (size_t i = 0; i < P; i++) { for (size_t j = 0; j < Q; j++) { @@ -140,9 +156,9 @@ class Slice return self; } - Slice &operator+=(const Type &other) + SliceT &operator+=(const Type &other) { - Slice &self = *this; + SliceT &self = *this; for (size_t i = 0; i < P; i++) { for (size_t j = 0; j < Q; j++) { @@ -154,9 +170,9 @@ class Slice } template - Slice &operator-=(const Slice &other) + SliceT &operator-=(const SliceT &other) { - Slice &self = *this; + SliceT &self = *this; for (size_t i = 0; i < P; i++) { for (size_t j = 0; j < Q; j++) { @@ -167,9 +183,9 @@ class Slice return self; } - Slice &operator-=(const Matrix &other) + SliceT &operator-=(const Matrix &other) { - Slice &self = *this; + SliceT &self = *this; for (size_t i = 0; i < P; i++) { for (size_t j = 0; j < Q; j++) { @@ -180,9 +196,9 @@ class Slice return self; } - Slice &operator-=(const Type &other) + SliceT &operator-=(const Type &other) { - Slice &self = *this; + SliceT &self = *this; for (size_t i = 0; i < P; i++) { for (size_t j = 0; j < Q; j++) { @@ -193,9 +209,9 @@ class Slice return self; } - Slice &operator*=(const Type &other) + SliceT &operator*=(const Type &other) { - Slice &self = *this; + SliceT &self = *this; for (size_t i = 0; i < P; i++) { for (size_t j = 0; j < Q; j++) { @@ -206,14 +222,14 @@ class Slice return self; } - Slice &operator/=(const Type &other) + SliceT &operator/=(const Type &other) { return operator*=(Type(1) / other); } Matrix operator*(const Type &other) const { - const Slice &self = *this; + const SliceT &self = *this; Matrix res; for (size_t i = 0; i < P; i++) { @@ -227,25 +243,25 @@ class Slice Matrix operator/(const Type &other) const { - const Slice &self = *this; + const SliceT &self = *this; return self * (Type(1) / other); } template - const Slice slice(size_t x0, size_t y0) const + const SliceT slice(size_t x0, size_t y0) const { - return Slice(x0 + _x0, y0 + _y0, _data); + return SliceT(x0 + _x0, y0 + _y0, _data); } template - Slice slice(size_t x0, size_t y0) + SliceT slice(size_t x0, size_t y0) { - return Slice(x0 + _x0, y0 + _y0, _data); + return SliceT(x0 + _x0, y0 + _y0, _data); } void copyTo(Type dst[P * Q]) const { - const Slice &self = *this; + const SliceT &self = *this; for (size_t i = 0; i < P; i++) { for (size_t j = 0; j < Q; j++) { @@ -256,7 +272,7 @@ class Slice void copyToColumnMajor(Type dst[P * Q]) const { - const Slice &self = *this; + const SliceT &self = *this; for (size_t i = 0; i < P; i++) { for (size_t j = 0; j < Q; j++) { @@ -267,7 +283,7 @@ class Slice Vector < Type, P < Q ? P : Q > diag() const { - const Slice &self = *this; + const SliceT &self = *this; Vector < Type, P < Q ? P : Q > res; for (size_t j = 0; j < (P < Q ? P : Q); j++) { @@ -279,7 +295,7 @@ class Slice Type norm_squared() const { - const Slice &self = *this; + const SliceT &self = *this; Type accum(0); for (size_t i = 0; i < P; i++) { @@ -337,7 +353,13 @@ class Slice private: size_t _x0, _y0; - Matrix *_data; + MatrixT *_data; }; +template +using Slice = SliceT, Type, P, Q, M, N>; + +template +using ConstSlice = SliceT, Type, P, Q, M, N>; + } diff --git a/src/lib/matrix/matrix/SparseVector.hpp b/src/lib/matrix/matrix/SparseVector.hpp index 40e790dfbf76..79fc360db63b 100644 --- a/src/lib/matrix/matrix/SparseVector.hpp +++ b/src/lib/matrix/matrix/SparseVector.hpp @@ -10,7 +10,7 @@ #pragma once -#include "math.hpp" +#include "Vector.hpp" namespace matrix { diff --git a/src/lib/matrix/matrix/SquareMatrix.hpp b/src/lib/matrix/matrix/SquareMatrix.hpp index ce3b37499266..76cb9a3ebc47 100644 --- a/src/lib/matrix/matrix/SquareMatrix.hpp +++ b/src/lib/matrix/matrix/SquareMatrix.hpp @@ -10,20 +10,11 @@ #include // FLT_EPSILON -#include "math.hpp" +#include "Slice.hpp" namespace matrix { -template -class Matrix; - -template -class Vector; - -template -class Slice; - template class SquareMatrix : public Matrix { @@ -45,10 +36,8 @@ class SquareMatrix : public Matrix { } - template - SquareMatrix(const Slice &in_slice) : Matrix(in_slice) - { - } + using base = Matrix; + using base::base; SquareMatrix &operator=(const Matrix &other) { @@ -64,15 +53,15 @@ class SquareMatrix : public Matrix } template - const Slice slice(size_t x0, size_t y0) const + ConstSlice slice(size_t x0, size_t y0) const { - return Slice(x0, y0, this); + return {x0, y0, this}; } template Slice slice(size_t x0, size_t y0) { - return Slice(x0, y0, this); + return {x0, y0, this}; } // inverse alias diff --git a/src/lib/matrix/matrix/Vector.hpp b/src/lib/matrix/matrix/Vector.hpp index dc80e28d2c8a..ba0d0b51ce86 100644 --- a/src/lib/matrix/matrix/Vector.hpp +++ b/src/lib/matrix/matrix/Vector.hpp @@ -8,14 +8,11 @@ #pragma once -#include "math.hpp" +#include "Matrix.hpp" namespace matrix { -template -class Matrix; - template class Vector : public Matrix { @@ -50,6 +47,22 @@ class Vector : public Matrix } } + template + Vector(const ConstSlice &slice_in) : + Matrix(slice_in) + { + } + + template + Vector(const ConstSlice &slice_in) + { + Vector &self(*this); + + for (size_t i = 0; i < M; i++) { + self(i) = slice_in(0, i); + } + } + inline const Type &operator()(size_t i) const { assert(i < M); diff --git a/src/lib/matrix/matrix/Vector2.hpp b/src/lib/matrix/matrix/Vector2.hpp index 35ceb7c1ac14..3b5bbadca8a7 100644 --- a/src/lib/matrix/matrix/Vector2.hpp +++ b/src/lib/matrix/matrix/Vector2.hpp @@ -8,14 +8,11 @@ #pragma once -#include "math.hpp" +#include "Vector.hpp" namespace matrix { -template -class Vector; - template class Vector2 : public Vector { @@ -43,15 +40,8 @@ class Vector2 : public Vector v(1) = y; } - template - Vector2(const Slice &slice_in) : Vector(slice_in) - { - } - - template - Vector2(const Slice &slice_in) : Vector(slice_in) - { - } + using base = Vector; + using base::base; explicit Vector2(const Vector3 &other) { diff --git a/src/lib/matrix/matrix/Vector3.hpp b/src/lib/matrix/matrix/Vector3.hpp index b67ab39a8bb9..ccfece92f0e4 100644 --- a/src/lib/matrix/matrix/Vector3.hpp +++ b/src/lib/matrix/matrix/Vector3.hpp @@ -8,23 +8,14 @@ #pragma once -#include "math.hpp" +#include "Vector.hpp" namespace matrix { -template -class Matrix; - -template -class Vector; - template class Dcm; -template -class Vector2; - template class Vector3 : public Vector { @@ -52,15 +43,8 @@ class Vector3 : public Vector v(2) = z; } - template - Vector3(const Slice &slice_in) : Vector(slice_in) - { - } - - template - Vector3(const Slice &slice_in) : Vector(slice_in) - { - } + using base = Vector; + using base::base; Vector3 cross(const Matrix31 &b) const { @@ -112,30 +96,16 @@ class Vector3 : public Vector return (*this).cross(b); } - /** - * Override vector ops so Vector3 type is returned - */ - inline Vector3 unit() const - { - return Vector3(Vector::unit()); - } - - inline Vector3 normalized() const + ConstSlice xy() const { - return unit(); - } - - const Slice xy() const - { - return Slice(0, 0, this); + return {0, 0, this}; } Slice xy() { - return Slice(0, 0, this); + return {0, 0, this}; } - Dcm hat() const // inverse to Dcm.vee() operation { const Vector3 &v(*this); diff --git a/src/lib/matrix/matrix/Vector4.hpp b/src/lib/matrix/matrix/Vector4.hpp index 088399847d98..4eba1de6394f 100644 --- a/src/lib/matrix/matrix/Vector4.hpp +++ b/src/lib/matrix/matrix/Vector4.hpp @@ -41,17 +41,11 @@ #pragma once -#include "math.hpp" +#include "Vector.hpp" namespace matrix { -template -class Matrix; - -template -class Vector; - template class Vector4 : public Vector { diff --git a/src/lib/matrix/matrix/filter.hpp b/src/lib/matrix/matrix/filter.hpp index 76c091d44aba..983af10eab2d 100644 --- a/src/lib/matrix/matrix/filter.hpp +++ b/src/lib/matrix/matrix/filter.hpp @@ -1,6 +1,7 @@ #pragma once -#include "math.hpp" +#include "Scalar.hpp" +#include "SquareMatrix.hpp" namespace matrix { diff --git a/src/lib/matrix/matrix/helper_functions.hpp b/src/lib/matrix/matrix/helper_functions.hpp index 08e74d7115ea..d482a92cf21f 100644 --- a/src/lib/matrix/matrix/helper_functions.hpp +++ b/src/lib/matrix/matrix/helper_functions.hpp @@ -2,7 +2,7 @@ #include -#include "math.hpp" +#include namespace matrix { diff --git a/src/lib/matrix/matrix/integration.hpp b/src/lib/matrix/matrix/integration.hpp index 338958ed4b46..9ca190333e6d 100644 --- a/src/lib/matrix/matrix/integration.hpp +++ b/src/lib/matrix/matrix/integration.hpp @@ -1,6 +1,6 @@ #pragma once -#include "math.hpp" +#include "Vector.hpp" namespace matrix { diff --git a/src/lib/matrix/matrix/math.hpp b/src/lib/matrix/matrix/math.hpp index dd80723d85c2..7f55520544ab 100644 --- a/src/lib/matrix/matrix/math.hpp +++ b/src/lib/matrix/matrix/math.hpp @@ -1,24 +1,20 @@ #pragma once -#include -#include - +#include "AxisAngle.hpp" +#include "Dcm.hpp" +#include "Dcm2.hpp" +#include "Dual.hpp" +#include "Euler.hpp" #include "helper_functions.hpp" - +#include "LeastSquaresSolver.hpp" #include "Matrix.hpp" -#include "SquareMatrix.hpp" +#include "PseudoInverse.hpp" +#include "Quaternion.hpp" +#include "Scalar.hpp" #include "Slice.hpp" +#include "SparseVector.hpp" +#include "SquareMatrix.hpp" #include "Vector.hpp" #include "Vector2.hpp" #include "Vector3.hpp" #include "Vector4.hpp" -#include "Euler.hpp" -#include "Dcm.hpp" -#include "Dcm2.hpp" -#include "Scalar.hpp" -#include "Quaternion.hpp" -#include "AxisAngle.hpp" -#include "LeastSquaresSolver.hpp" -#include "Dual.hpp" -#include "PseudoInverse.hpp" -#include "SparseVector.hpp" diff --git a/src/lib/matrix/test/MatrixAssignmentTest.cpp b/src/lib/matrix/test/MatrixAssignmentTest.cpp index 29b7f9951d33..0500f8734f55 100644 --- a/src/lib/matrix/test/MatrixAssignmentTest.cpp +++ b/src/lib/matrix/test/MatrixAssignmentTest.cpp @@ -276,14 +276,20 @@ TEST(MatrixAssignmentTest, Assignment) } } + char print_out[] = " | 0 | 1 \n 0| 1.00000 1.2e+04\n 1| 1.2e+04 0.12346\n 2| 1.2e+10 1.2e+12\n"; + printf("%s\n", print_out); // for debugging in case of failure + // check print() // Redirect stdout - EXPECT_TRUE(freopen("testoutput.txt", "w", stdout) != NULL); + FILE *fp = freopen("testoutput.txt", "w", stdout); + EXPECT_NE(fp, nullptr); + // write Comma.print(); - fclose(stdout); + EXPECT_FALSE(fclose(fp)); // FIXME: this doesn't work as expected, further printf are not redirected to the console + // read - FILE *fp = fopen("testoutput.txt", "r"); + fp = fopen("testoutput.txt", "r"); EXPECT_NE(fp, nullptr); EXPECT_FALSE(fseek(fp, 0, SEEK_SET)); @@ -294,8 +300,8 @@ TEST(MatrixAssignmentTest, Assignment) break; } - printf("%d %d %d\n", static_cast(i), output[i], c); - EXPECT_EQ(c, output[i]); + printf("%d %d %d\n", static_cast(i), print_out[i], c); + EXPECT_EQ(c, print_out[i]); } EXPECT_FALSE(fclose(fp)); diff --git a/src/lib/matrix/test/MatrixDcm2Test.cpp b/src/lib/matrix/test/MatrixDcm2Test.cpp index 32cdc3608583..4e1b696ae6bb 100644 --- a/src/lib/matrix/test/MatrixDcm2Test.cpp +++ b/src/lib/matrix/test/MatrixDcm2Test.cpp @@ -34,6 +34,7 @@ #include #include #include +#include using namespace matrix; diff --git a/src/lib/matrix/test/MatrixFilterTest.cpp b/src/lib/matrix/test/MatrixFilterTest.cpp index 10d1b786ae17..a5104c7daaf5 100644 --- a/src/lib/matrix/test/MatrixFilterTest.cpp +++ b/src/lib/matrix/test/MatrixFilterTest.cpp @@ -33,6 +33,7 @@ #include #include +#include using namespace matrix; diff --git a/src/lib/mixer_module/functions/FunctionActuatorSet.hpp b/src/lib/mixer_module/functions/FunctionActuatorSet.hpp index a132304bf005..8c34767eb5d9 100644 --- a/src/lib/mixer_module/functions/FunctionActuatorSet.hpp +++ b/src/lib/mixer_module/functions/FunctionActuatorSet.hpp @@ -61,12 +61,17 @@ class FunctionActuatorSet : public FunctionProviderBase int index = (int)(vehicle_command.param7 + 0.5f); if (index == 0) { - _data[0] = vehicle_command.param1; - _data[1] = vehicle_command.param2; - _data[2] = vehicle_command.param3; - _data[3] = vehicle_command.param4; - _data[4] = vehicle_command.param5; - _data[5] = vehicle_command.param6; + if (PX4_ISFINITE(vehicle_command.param1)) { _data[0] = vehicle_command.param1; } + + if (PX4_ISFINITE(vehicle_command.param2)) {_data[1] = vehicle_command.param2; } + + if (PX4_ISFINITE(vehicle_command.param3)) {_data[2] = vehicle_command.param3; } + + if (PX4_ISFINITE(vehicle_command.param4)) {_data[3] = vehicle_command.param4; } + + if (PX4_ISFINITE(vehicle_command.param5)) {_data[4] = vehicle_command.param5; } + + if (PX4_ISFINITE(vehicle_command.param6)) {_data[5] = vehicle_command.param6; } } } } diff --git a/src/lib/parameters/CMakeLists.txt b/src/lib/parameters/CMakeLists.txt index a68a6e38adfb..b87e652cad4f 100644 --- a/src/lib/parameters/CMakeLists.txt +++ b/src/lib/parameters/CMakeLists.txt @@ -156,7 +156,23 @@ add_custom_target(parameters_header DEPENDS px4_parameters.hpp) set(SRCS) -list(APPEND SRCS parameters.cpp atomic_transaction.cpp autosave.cpp) +list(APPEND SRCS + parameters.cpp + atomic_transaction.cpp + autosave.cpp +) + +if(CONFIG_PARAM_PRIMARY) +list(APPEND SRCS + parameters_primary.cpp +) +endif() + +if(CONFIG_PARAM_REMOTE) +list(APPEND SRCS + parameters_remote.cpp +) +endif() if(BUILD_TESTING) list(APPEND SRCS param_translation_unit_tests.cpp) diff --git a/src/lib/parameters/Kconfig b/src/lib/parameters/Kconfig new file mode 100644 index 000000000000..f296180d996f --- /dev/null +++ b/src/lib/parameters/Kconfig @@ -0,0 +1,11 @@ +menuconfig PARAM_PRIMARY + bool "parameter primary" + default n + ---help--- + Enable support for the parameter primary in distributed board architectures + +menuconfig PARAM_REMOTE + bool "parameter remote" + default n + ---help--- + Enable support for the parameter remote in distributed board architectures diff --git a/src/lib/parameters/flashparams/flashparams.cpp b/src/lib/parameters/flashparams/flashparams.cpp index f466fd805e9f..c37959a706fc 100644 --- a/src/lib/parameters/flashparams/flashparams.cpp +++ b/src/lib/parameters/flashparams/flashparams.cpp @@ -189,7 +189,9 @@ param_import_callback(bson_decoder_t decoder, bson_node_t node) return 0; } - param_modify_on_import(node); + if (param_modify_on_import(node) == param_modify_on_import_ret::PARAM_SKIP_IMPORT) { + return 1; + } /* * Find the parameter this node represents. If we don't know it, diff --git a/src/lib/parameters/param.h b/src/lib/parameters/param.h index dca29d4f212f..c8b8dbd79df9 100644 --- a/src/lib/parameters/param.h +++ b/src/lib/parameters/param.h @@ -267,6 +267,18 @@ __EXPORT void param_set_used(param_t param); */ __EXPORT int param_set_no_notification(param_t param, const void *val); +/** + * Set the value of a parameter, but do not update the remote system. This avoids + * a set loop between primary and remote. + * + * @param param A handle returned by param_find or passed by param_foreach. + * @param val The value to set; assumed to point to a variable of the parameter type. + * For structures, the pointer is assumed to point to a structure to be copied. + * @param notify Set this to true for the primary (to send out a param update) and false on client + * @return Zero if the parameter's value could be set from a scalar, nonzero otherwise. + */ +__EXPORT int param_set_no_remote_update(param_t param, const void *val, bool notify); + /** * Notify the system about parameter changes. Can be used for example after several calls to * param_set_no_notification() to avoid unnecessary system notifications. diff --git a/src/lib/parameters/param_translation.cpp b/src/lib/parameters/param_translation.cpp index 4f83be01aa2c..93cbddcdec42 100644 --- a/src/lib/parameters/param_translation.cpp +++ b/src/lib/parameters/param_translation.cpp @@ -40,7 +40,7 @@ #include #include -bool param_modify_on_import(bson_node_t node) +param_modify_on_import_ret param_modify_on_import(bson_node_t node) { // 2023-12-06: translate and invert FW_ARSP_MODE-> FW_USE_AIRSPD { @@ -54,7 +54,7 @@ bool param_modify_on_import(bson_node_t node) strcpy(node->name, "FW_USE_AIRSPD"); PX4_INFO("copying and inverting %s -> %s", "FW_ARSP_MODE", "FW_USE_AIRSPD"); - return true; + return param_modify_on_import_ret::PARAM_MODIFIED; } } @@ -69,9 +69,9 @@ bool param_modify_on_import(bson_node_t node) } - return true; + return param_modify_on_import_ret::PARAM_MODIFIED; } } - return false; + return param_modify_on_import_ret::PARAM_NOT_MODIFIED; } diff --git a/src/lib/parameters/param_translation.h b/src/lib/parameters/param_translation.h index 1d001ce09e06..2bbf401ac70f 100644 --- a/src/lib/parameters/param_translation.h +++ b/src/lib/parameters/param_translation.h @@ -35,4 +35,10 @@ #include "tinybson/tinybson.h" -__EXPORT bool param_modify_on_import(bson_node_t node); +enum class param_modify_on_import_ret { + PARAM_SKIP_IMPORT = 0, + PARAM_NOT_MODIFIED = 1, + PARAM_MODIFIED = 2 +}; + +__EXPORT param_modify_on_import_ret param_modify_on_import(bson_node_t node); diff --git a/src/lib/parameters/param_translation_unit_tests.cpp b/src/lib/parameters/param_translation_unit_tests.cpp index 69709bf31afb..b2e398effb34 100644 --- a/src/lib/parameters/param_translation_unit_tests.cpp +++ b/src/lib/parameters/param_translation_unit_tests.cpp @@ -33,8 +33,8 @@ #include "param_translation.h" -bool param_modify_on_import(bson_node_t node) +param_modify_on_import_ret param_modify_on_import(bson_node_t node) { // don't modify params for unit tests - return false; + return param_modify_on_import_ret::PARAM_NOT_MODIFIED; } diff --git a/src/lib/parameters/parameters.cpp b/src/lib/parameters/parameters.cpp index ad35dca782bc..f8cd3d6fa3ea 100644 --- a/src/lib/parameters/parameters.cpp +++ b/src/lib/parameters/parameters.cpp @@ -105,8 +105,10 @@ static DynamicSparseLayer runtime_defaults{&firmware_defaults}; DynamicSparseLayer user_config{&runtime_defaults}; /** parameter update topic handle */ +#if not defined(CONFIG_PARAM_REMOTE) static orb_advert_t param_topic = nullptr; static unsigned int param_instance = 0; +#endif static perf_counter_t param_export_perf; static perf_counter_t param_find_perf; @@ -116,6 +118,14 @@ static perf_counter_t param_set_perf; static pthread_mutex_t file_mutex = PTHREAD_MUTEX_INITIALIZER; ///< this protects against concurrent param saves (file or flash access). +// Support for remote parameter node +#if defined(CONFIG_PARAM_PRIMARY) +# include "parameters_primary.h" +#endif // CONFIG_PARAM_PRIMARY +#if defined(CONFIG_PARAM_REMOTE) +# include "parameters_remote.h" +#endif // CONFIG_PARAM_REMOTE + void param_init() { @@ -128,14 +138,27 @@ param_init() px4_register_boardct_ioctl(_PARAMIOCBASE, param_ioctl); #endif +#if defined(CONFIG_PARAM_PRIMARY) + param_primary_init(); +#endif // CONFIG_PARAM_PRIMARY + +#if defined(CONFIG_PARAM_REMOTE) + param_remote_init(); +#endif // CONFIG_PARAM_REMOTE + +#if not defined(CONFIG_PARAM_REMOTE) autosave_instance = new ParamAutosave(); +#endif } void param_notify_changes() { - parameter_update_s pup{}; +// Don't send if this is a remote node. Only the primary +// sends out update notices +#if not defined(CONFIG_PARAM_REMOTE) + parameter_update_s pup {}; pup.instance = param_instance++; pup.get_count = perf_event_count(param_get_perf); pup.set_count = perf_event_count(param_set_perf); @@ -152,6 +175,8 @@ param_notify_changes() } else { orb_publish(ORB_ID(parameter_update), param_topic, &pup); } + +#endif } static param_t param_find_internal(const char *name, bool notification) @@ -372,7 +397,7 @@ param_control_autosave(bool enable) } static int -param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_changes) +param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_changes, bool update_remote = true) { if (!handle_in_range(param)) { PX4_ERR("set invalid param %d", param); @@ -421,6 +446,24 @@ param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_ param_autosave(); } + // If this is the parameter server, make sure that the remote is updated +#if defined(CONFIG_PARAM_PRIMARY) + + if (param_changed && update_remote) { + param_primary_set_value(param, val); + } + +#endif + + // If this is the parameter remote, make sure that the primary is updated +#if defined(CONFIG_PARAM_REMOTE) + + if (param_changed && update_remote) { + param_remote_set_value(param, val); + } + +#endif + perf_end(param_set_perf); /* @@ -456,6 +499,11 @@ int param_set_no_notification(param_t param, const void *val) return param_set_internal(param, val, false, false); } +int param_set_no_remote_update(param_t param, const void *val, bool notify) +{ + return param_set_internal(param, val, false, notify, false); +} + bool param_used(param_t param) { if (handle_in_range(param)) { @@ -468,6 +516,14 @@ bool param_used(param_t param) void param_set_used(param_t param) { if (handle_in_range(param)) { +#if defined(CONFIG_PARAM_REMOTE) + + if (!param_used(param)) { + param_remote_set_used(param); + } + +#endif + params_active.set(param, true); } } @@ -544,6 +600,11 @@ int param_set_default_value(param_t param, const void *val) static int param_reset_internal(param_t param, bool notify = true, bool autosave = true) { +#if defined(CONFIG_PARAM_REMOTE) + // Remote doesn't support reset + return false; +#endif + bool param_found = user_config.contains(param); if (handle_in_range(param)) { @@ -558,6 +619,10 @@ static int param_reset_internal(param_t param, bool notify = true, bool autosave param_notify_changes(); } +#if defined(CONFIG_PARAM_PRIMARY) + param_primary_reset(param); +#endif + return param_found; } @@ -567,6 +632,11 @@ int param_reset_no_notification(param_t param) { return param_reset_internal(par static void param_reset_all_internal(bool auto_save) { +#if defined(CONFIG_PARAM_REMOTE) + // Remote doesn't support reset + return; +#endif + for (param_t param = 0; handle_in_range(param); param++) { param_reset_internal(param, false, false); } @@ -575,6 +645,10 @@ param_reset_all_internal(bool auto_save) param_autosave(); } +#if defined(CONFIG_PARAM_PRIMARY) + param_primary_reset_all(); +#endif + param_notify_changes(); } @@ -1078,7 +1152,10 @@ param_import_callback(bson_decoder_t decoder, bson_node_t node) return 0; } - param_modify_on_import(node); + // if we do param_set() directly in the translation, set PARAM_SKIP_IMPORT as return value and return here + if (param_modify_on_import(node) == param_modify_on_import_ret::PARAM_SKIP_IMPORT) { + return 1; + } // Find the parameter this node represents. If we don't know it, ignore the node. param_t param = param_find_no_notification(node->name); @@ -1277,4 +1354,27 @@ void param_print_status() perf_print_counter(param_find_perf); perf_print_counter(param_get_perf); perf_print_counter(param_set_perf); + +#if defined(CONFIG_PARAM_PRIMARY) + struct param_primary_counters counts; + param_primary_get_counters(&counts); + PX4_INFO("set value requests received: %" PRIu32 ", set value responses sent: %" PRIu32, + counts.set_value_request_received, counts.set_value_response_sent); + PX4_INFO("set value requests sent: %" PRIu32 ", set value responses received: %" PRIu32, + counts.set_value_request_sent, counts.set_value_response_received); + PX4_INFO("resets sent: %" PRIu32 ", set used requests received: %" PRIu32, + counts.reset_sent, counts.set_used_received); +#endif + +#if defined(CONFIG_PARAM_REMOTE) + struct param_remote_counters counts; + param_remote_get_counters(&counts); + PX4_INFO("set value requests received: %" PRIu32 ", set value responses sent: %" PRIu32, + counts.set_value_request_received, counts.set_value_response_sent); + PX4_INFO("set value requests sent: %" PRIu32 ", set value responses received: %" PRIu32, + counts.set_value_request_sent, counts.set_value_response_received); + PX4_INFO("resets received: %" PRIu32 ", set used requests sent: %" PRIu32, + counts.reset_received, counts.set_used_sent); +#endif + } diff --git a/src/lib/parameters/parameters_primary.cpp b/src/lib/parameters/parameters_primary.cpp new file mode 100644 index 000000000000..02de11e608d0 --- /dev/null +++ b/src/lib/parameters/parameters_primary.cpp @@ -0,0 +1,314 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "parameters_primary.h" + +#include "uORB/uORBManager.hpp" + +#include +#include + +// uORB topics needed to keep parameter server and client in sync +#include +#include +#include +#include + +// Debug flag +static bool debug = false; + +static struct param_primary_counters param_primary_counters; + +#define TIMEOUT_WAIT 1000 +#define TIMEOUT_COUNT 50 + +static px4_task_t sync_thread_tid; +static const char *sync_thread_name = "param_primary_sync"; + +static orb_advert_t param_set_value_req_h = nullptr; +static orb_advert_t param_reset_req_h = nullptr; + +static int param_set_rsp_fd = PX4_ERROR; + +static int primary_sync_thread(int argc, char *argv[]) +{ + // Need to wait until the uORB and muORB are ready + // Check for uORB initialization with get_instance + while (uORB::Manager::get_instance() == nullptr) { px4_usleep(100); } + + // Check for muORB initialization with get_uorb_communicator + while (uORB::Manager::get_instance()->get_uorb_communicator() == nullptr) { px4_usleep(100); } + + orb_advert_t _set_value_rsp_h = nullptr; + + int _set_used_req_fd = orb_subscribe(ORB_ID(parameter_set_used_request)); + int _set_value_req_fd = orb_subscribe(ORB_ID(parameter_primary_set_value_request)); + + struct parameter_set_used_request_s _set_used_request; + struct parameter_set_value_request_s _set_value_request; + struct parameter_set_value_response_s _set_value_response; + + px4_pollfd_struct_t fds[2] = { { .fd = _set_used_req_fd, .events = POLLIN }, + { .fd = _set_value_req_fd, .events = POLLIN } + }; + + PX4_INFO("Starting parameter primary sync thread"); + + while (true) { + px4_poll(fds, 2, 1000); + + if (fds[0].revents & POLLIN) { + bool updated = true; + + while (updated) { + orb_copy(ORB_ID(parameter_set_used_request), _set_used_req_fd, &_set_used_request); + + if (debug) { + PX4_INFO("Got parameter_set_used_request for %s", param_name(_set_used_request.parameter_index)); + } + + param_primary_counters.set_used_received++; + + param_find(param_name(_set_used_request.parameter_index)); + + (void) orb_check(_set_used_req_fd, &updated); + } + + } + + if (fds[1].revents & POLLIN) { + bool updated = true; + + while (updated) { + orb_copy(ORB_ID(parameter_primary_set_value_request), _set_value_req_fd, &_set_value_request); + + if (debug) { + PX4_INFO("Got parameter_primary_set_value_request for %s", param_name(_set_value_request.parameter_index)); + } + + param_primary_counters.set_value_request_received++; + + param_t param = _set_value_request.parameter_index; + + switch (param_type(param)) { + case PARAM_TYPE_INT32: + param_set_no_remote_update(param, (const void *) &_set_value_request.int_value, true); + break; + + case PARAM_TYPE_FLOAT: + param_set_no_remote_update(param, (const void *) &_set_value_request.float_value, true); + break; + + default: + PX4_ERR("Parameter must be either int or float"); + break; + } + + _set_value_response.timestamp = hrt_absolute_time(); + _set_value_response.request_timestamp = _set_value_request.timestamp; + _set_value_response.parameter_index = _set_value_request.parameter_index; + + if (_set_value_rsp_h == nullptr) { + _set_value_rsp_h = orb_advertise(ORB_ID(parameter_primary_set_value_response), &_set_value_response); + + } else { + orb_publish(ORB_ID(parameter_primary_set_value_response), _set_value_rsp_h, &_set_value_response); + } + + param_primary_counters.set_value_response_sent++; + + (void) orb_check(_set_value_req_fd, &updated); + } + } + } + + return 0; +} + +void param_primary_init() +{ + + sync_thread_tid = px4_task_spawn_cmd(sync_thread_name, + SCHED_DEFAULT, + SCHED_PRIORITY_PARAMS, + (1024 * 4), + primary_sync_thread, + NULL); + +} + +// void param_primary_set_value(param_t param, const void *val, bool from_file) +void param_primary_set_value(param_t param, const void *val) +{ + bool send_request = true; + struct parameter_set_value_request_s req; + req.timestamp = hrt_absolute_time(); + req.parameter_index = param; + + switch (param_type(param)) { + case PARAM_TYPE_INT32: + req.int_value = *(int32_t *)val; + + if (debug) { + PX4_INFO("*** Setting %s to %d ***", param_name(req.parameter_index), req.int_value); + } + + break; + + case PARAM_TYPE_FLOAT: + req.float_value = *(float *)val; + + if (debug) { + PX4_INFO("*** Setting %s to %f ***", param_name(req.parameter_index), (double) req.float_value); + } + + break; + + default: + PX4_ERR("Parameter must be either int or float"); + send_request = false; + break; + } + + if (param_set_rsp_fd == PX4_ERROR) { + if (debug) { + PX4_INFO("Subscribing to parameter_client_set_value_response"); + } + + param_set_rsp_fd = orb_subscribe(ORB_ID(parameter_remote_set_value_response)); + + if (param_set_rsp_fd == PX4_ERROR) { + PX4_ERR("Subscription to parameter_remote_set_value_response failed"); + + } else { + if (debug) { + PX4_INFO("Subscription to parameter_client_set_value_response succeeded"); + } + } + } + + if (send_request) { + if (debug) { + PX4_INFO("Sending param set request to remote for %s", param_name(req.parameter_index)); + } + + if (param_set_value_req_h == nullptr) { + param_set_value_req_h = orb_advertise(ORB_ID(parameter_remote_set_value_request), nullptr); + } + + orb_publish(ORB_ID(parameter_remote_set_value_request), param_set_value_req_h, &req); + + param_primary_counters.set_value_request_sent++; + + // Wait for response + bool updated = false; + + if (debug) { + PX4_INFO("Waiting for parameter_client_set_value_response for %s", param_name(req.parameter_index)); + } + + px4_usleep(TIMEOUT_WAIT); + int count = TIMEOUT_COUNT; + + while (--count) { + (void) orb_check(param_set_rsp_fd, &updated); + + struct parameter_set_value_response_s rsp; + + while (updated) { + + orb_copy(ORB_ID(parameter_remote_set_value_response), param_set_rsp_fd, &rsp); + + if ((rsp.request_timestamp == req.timestamp) && (rsp.parameter_index == req.parameter_index)) { + if (debug) { + PX4_INFO("Got parameter_remote_set_value_response for %s", param_name(req.parameter_index)); + } + + param_primary_counters.set_value_response_received++; + return; + } + + (void) orb_check(param_set_rsp_fd, &updated); + } + + px4_usleep(TIMEOUT_WAIT); + } + + PX4_ERR("Timeout waiting for parameter_client_set_value_response for %s", param_name(req.parameter_index)); + } +} + +static void param_primary_reset_internal(param_t param, bool reset_all) +{ + if (debug) { + PX4_INFO("Param reset at primary"); + } + + struct parameter_reset_request_s req; + + req.timestamp = hrt_absolute_time(); + + req.reset_all = reset_all; + + if (reset_all == false) { + req.parameter_index = param; + } + + if (debug) { + PX4_INFO("Sending param reset request to remote"); + } + + if (param_reset_req_h == nullptr) { + param_reset_req_h = orb_advertise(ORB_ID(parameter_reset_request), &req); + + } else { + orb_publish(ORB_ID(parameter_reset_request), param_reset_req_h, &req); + } + + param_primary_counters.reset_sent++; +} + +void param_primary_reset(param_t param) +{ + param_primary_reset_internal(param, false); +} + +void param_primary_reset_all() +{ + param_primary_reset_internal(0, true); +} + +void param_primary_get_counters(struct param_primary_counters *cnt) +{ + *cnt = param_primary_counters; +} \ No newline at end of file diff --git a/src/lib/parameters/parameters_primary.h b/src/lib/parameters/parameters_primary.h new file mode 100644 index 000000000000..7994822d4c05 --- /dev/null +++ b/src/lib/parameters/parameters_primary.h @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "param.h" + +struct param_primary_counters { + uint32_t set_value_request_received; + uint32_t set_value_response_sent; + uint32_t reset_sent; + uint32_t set_value_request_sent; + uint32_t set_value_response_received; + uint32_t set_used_received; +}; + +void param_primary_init(); +void param_primary_set_value(param_t param, const void *val); +void param_primary_reset(param_t param); +void param_primary_reset_all(); +void param_primary_get_counters(struct param_primary_counters *cnt); + diff --git a/src/lib/parameters/parameters_remote.cpp b/src/lib/parameters/parameters_remote.cpp new file mode 100644 index 000000000000..78b7ee153cf1 --- /dev/null +++ b/src/lib/parameters/parameters_remote.cpp @@ -0,0 +1,310 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "parameters_remote.h" + +#include "uORB/uORBManager.hpp" + +#include + +#include +#include + +// uORB topics needed to keep parameter server and client in sync +#include +#include +#include +#include + +// Debug flag +static bool debug = false; + +static struct param_remote_counters param_remote_counters; + +#define TIMEOUT_WAIT 1000 +#define TIMEOUT_COUNT 50 + +static orb_advert_t parameter_set_used_h = nullptr; +static orb_advert_t param_set_value_req_h = nullptr; + +static int param_set_rsp_fd = PX4_ERROR; + +static px4_task_t sync_thread_tid; +static const char *sync_thread_name = "param_remote_sync"; + +static int remote_sync_thread(int argc, char *argv[]) +{ + // This thread gets started by the remote side during PX4 initialization. + // We cannot send out the subscribe request immediately because the other + // side will not be ready to receive it on the muorb yet and it will get dropped. + // So, sleep a little bit to give other side a chance to finish initialization + // of the muorb. But don't wait too long otherwise a set request from the server + // side could be missed. + usleep(200000); + + orb_advert_t _set_value_rsp_h = nullptr; + + int _reset_req_fd = orb_subscribe(ORB_ID(parameter_reset_request)); + int _set_value_req_fd = orb_subscribe(ORB_ID(parameter_remote_set_value_request)); + + struct parameter_reset_request_s _reset_request; + struct parameter_set_value_request_s _set_value_request; + struct parameter_set_value_response_s _set_value_response; + + px4_pollfd_struct_t fds[2] = { { .fd = _reset_req_fd, .events = POLLIN }, + { .fd = _set_value_req_fd, .events = POLLIN } + }; + + PX4_INFO("Starting parameter remote sync thread"); + + while (true) { + px4_poll(fds, 2, 1000); + + if (fds[0].revents & POLLIN) { + bool updated = true; + + while (updated) { + orb_copy(ORB_ID(parameter_reset_request), _reset_req_fd, &_reset_request); + + if (debug) { + PX4_INFO("Got parameter_reset_request for %s", param_name(_reset_request.parameter_index)); + } + + param_remote_counters.reset_received++; + + if (_reset_request.reset_all) { + param_reset_all(); + + } else { + param_reset_no_notification(_reset_request.parameter_index); + + } + + (void) orb_check(_reset_req_fd, &updated); + } + } + + if (fds[1].revents & POLLIN) { + bool updated = true; + + while (updated) { + orb_copy(ORB_ID(parameter_primary_set_value_request), _set_value_req_fd, &_set_value_request); + + if (debug) { + PX4_INFO("Got parameter_remote_set_value_request for %s", param_name(_set_value_request.parameter_index)); + } + + param_remote_counters.set_value_request_received++; + + switch (param_type(_set_value_request.parameter_index)) { + case PARAM_TYPE_INT32: + param_set_no_remote_update(_set_value_request.parameter_index, + (const void *) &_set_value_request.int_value, + false); + break; + + case PARAM_TYPE_FLOAT: + param_set_no_remote_update(_set_value_request.parameter_index, + (const void *) &_set_value_request.float_value, + false); + break; + + default: + PX4_ERR("Parameter must be either int or float"); + break; + } + + _set_value_response.timestamp = hrt_absolute_time(); + _set_value_response.request_timestamp = _set_value_request.timestamp; + _set_value_response.parameter_index = _set_value_request.parameter_index; + + if (_set_value_rsp_h == nullptr) { + _set_value_rsp_h = orb_advertise(ORB_ID(parameter_remote_set_value_response), &_set_value_response); + + } else { + if (debug) { + PX4_INFO("Sending set value response for %s", param_name(_set_value_request.parameter_index)); + } + + orb_publish(ORB_ID(parameter_remote_set_value_response), _set_value_rsp_h, &_set_value_response); + } + + param_remote_counters.set_value_response_sent++; + + (void) orb_check(_set_value_req_fd, &updated); + } + } + } + + return 0; +} + +void param_remote_init() +{ + + sync_thread_tid = px4_task_spawn_cmd(sync_thread_name, + SCHED_DEFAULT, + SCHED_PRIORITY_PARAMS, + (1024 * 4), + remote_sync_thread, + NULL); + +} + +void param_remote_set_used(param_t param) +{ + // Notify the parameter server that this parameter has been marked as used + if (debug) { + PX4_INFO("Requesting server to mark %s as used", param_name(param)); + } + + struct parameter_set_used_request_s req; + + req.timestamp = hrt_absolute_time(); + + req.parameter_index = param; + + if (parameter_set_used_h == nullptr) { + parameter_set_used_h = orb_advertise(ORB_ID(parameter_set_used_request), &req); + + } else { + orb_publish(ORB_ID(parameter_set_used_request), parameter_set_used_h, &req); + } + + param_remote_counters.set_used_sent++; +} + +void param_remote_set_value(param_t param, const void *val) +{ + bool send_request = true; + struct parameter_set_value_request_s req; + req.timestamp = hrt_absolute_time(); + req.parameter_index = param; + + switch (param_type(param)) { + case PARAM_TYPE_INT32: + req.int_value = *(int32_t *)val; + + if (debug) { + PX4_INFO("*** Setting %s to %" PRIi32 " ***", param_name(req.parameter_index), req.int_value); + } + + break; + + case PARAM_TYPE_FLOAT: + req.float_value = *(float *)val; + + if (debug) { + PX4_INFO("*** Setting %s to %f ***", param_name(req.parameter_index), (double) req.float_value); + } + + break; + + default: + PX4_ERR("Parameter must be either int or float"); + send_request = false; + break; + } + + if (param_set_rsp_fd == PX4_ERROR) { + if (debug) { + PX4_INFO("Subscribing to parameter_primary_set_value_response"); + } + + param_set_rsp_fd = orb_subscribe(ORB_ID(parameter_primary_set_value_response)); + + if (param_set_rsp_fd == PX4_ERROR) { + PX4_ERR("Subscription to parameter_primary_set_value_response failed"); + + } else { + if (debug) { + PX4_INFO("Subscription to parameter_primary_set_value_response succeeded"); + } + } + } + + if (send_request) { + if (debug) { + PX4_INFO("Sending param set value request to primary for %s", param_name(req.parameter_index)); + } + + if (param_set_value_req_h == nullptr) { + param_set_value_req_h = orb_advertise(ORB_ID(parameter_primary_set_value_request), nullptr); + } + + orb_publish(ORB_ID(parameter_primary_set_value_request), param_set_value_req_h, &req); + + param_remote_counters.set_value_request_sent++; + + // Wait for response + bool updated = false; + + if (debug) { + PX4_INFO("Waiting for parameter_client_set_value_response for %s", param_name(req.parameter_index)); + } + + px4_usleep(TIMEOUT_WAIT); + int count = TIMEOUT_COUNT; + + while (--count) { + (void) orb_check(param_set_rsp_fd, &updated); + + struct parameter_set_value_response_s rsp; + + while (updated) { + + orb_copy(ORB_ID(parameter_primary_set_value_response), param_set_rsp_fd, &rsp); + + if ((rsp.request_timestamp == req.timestamp) && (rsp.parameter_index == req.parameter_index)) { + if (debug) { + PX4_INFO("Got parameter_primary_set_value_response for %s", param_name(req.parameter_index)); + } + + param_remote_counters.set_value_response_received++; + return; + } + + (void) orb_check(param_set_rsp_fd, &updated); + } + + px4_usleep(TIMEOUT_WAIT); + } + + PX4_ERR("Timeout waiting for parameter_primary_set_value_response for %s", param_name(req.parameter_index)); + } +} + +void param_remote_get_counters(struct param_remote_counters *cnt) +{ + *cnt = param_remote_counters; +} diff --git a/src/lib/parameters/parameters_remote.h b/src/lib/parameters/parameters_remote.h new file mode 100644 index 000000000000..7eba416643cf --- /dev/null +++ b/src/lib/parameters/parameters_remote.h @@ -0,0 +1,50 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "param.h" + +struct param_remote_counters { + uint32_t set_value_request_received; + uint32_t set_value_response_sent; + uint32_t reset_received; + uint32_t set_value_request_sent; + uint32_t set_value_response_received; + uint32_t set_used_sent; +}; + +void param_remote_init(); +void param_remote_set_used(param_t param); +void param_remote_set_value(param_t param, const void *val); +void param_remote_get_counters(struct param_remote_counters *cnt); diff --git a/src/lib/parameters/px4params/injectxmlparams.py b/src/lib/parameters/px4params/injectxmlparams.py index 060015a170ab..fd9d19ed178a 100755 --- a/src/lib/parameters/px4params/injectxmlparams.py +++ b/src/lib/parameters/px4params/injectxmlparams.py @@ -14,7 +14,7 @@ class XMLInject(): def __init__(self, injected_xml_filename): self.groups=[] - valid_parameter_attributes = set(["category", "default", "name", "type", "volatile"]) + valid_parameter_attributes = set(["category", "default", "name", "type", "volatile", "boolean"]) valid_field_tags = set(["board","short_desc", "long_desc", "min", "max", "unit", "decimal", "increment", "reboot_required"]) valid_other_top_level_tags = set(["group","values"]) @@ -42,7 +42,8 @@ def __init__(self, injected_xml_filename): new_param.default = iparam.get('default') elif param_attrib == 'volatile': new_param.SetVolatile() - + elif param_attrib == "boolean": + new_param.SetBoolean() #get param info stored as child tags for child in iparam: diff --git a/src/lib/parameters/px4params/srcparser.py b/src/lib/parameters/px4params/srcparser.py index 0e2c0b0ef9b5..a2fdf3705dca 100644 --- a/src/lib/parameters/px4params/srcparser.py +++ b/src/lib/parameters/px4params/srcparser.py @@ -358,7 +358,7 @@ def Validate(self): 'deg', 'deg*1e7', 'deg/s', 'deg/s^2', 'celcius', 'gauss', 'gauss/s', 'gauss^2', 'hPa', 'kg', 'kg/m^2', 'kg m^2', 'kg/m^3', - 'mm', 'm', 'm/s', 'm^2', 'm/s^2', 'm/s^3', 'm/s^2/sqrt(Hz)', '1/s/sqrt(Hz)', 'm/s/rad', + 'mm', 'm', 'm/s', 'm^2', 'm/s^2', 'm/s^3', 'm/s^2/sqrt(Hz)', '1/s/sqrt(Hz)', 'm/s/rad', 'g0', 'Ohm', 'V', 'A', 'us', 'ms', 's', 'S', 'A/%', '(m/s^2)^2', 'm/m', 'tan(rad)^2', '(m/s)^2', 'm/rad', diff --git a/src/lib/rtl/CMakeLists.txt b/src/lib/rtl/CMakeLists.txt new file mode 100644 index 000000000000..4f3e7a3c5331 --- /dev/null +++ b/src/lib/rtl/CMakeLists.txt @@ -0,0 +1,34 @@ +############################################################################ +# +# Copyright (c) 2024 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(rtl_time_estimator rtl_time_estimator.cpp) diff --git a/src/lib/rtl/rtl_time_estimator.cpp b/src/lib/rtl/rtl_time_estimator.cpp new file mode 100644 index 000000000000..055b62ccabca --- /dev/null +++ b/src/lib/rtl/rtl_time_estimator.cpp @@ -0,0 +1,267 @@ +/**************************************************************************** + * + * Copyright (C) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file rtl_time_estimator.cpp + * + * Helper class to calculate the remaining time estimate to go to RTL landing point. + * + */ + +#include "rtl_time_estimator.h" + +#include +#include + +#include +#include +#include + +RtlTimeEstimator::RtlTimeEstimator() : ModuleParams(nullptr) +{ + _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"); +}; + +rtl_time_estimate_s RtlTimeEstimator::getEstimate() const +{ + rtl_time_estimate_s time_estimate{}; + + if (_is_valid && PX4_ISFINITE(_time_estimate)) { + time_estimate.valid = true; + time_estimate.time_estimate = _time_estimate; + // Use actual time estimate to compute the safer time estimate with additional scale factor and a margin + time_estimate.safe_time_estimate = _param_rtl_time_factor.get() * _time_estimate + _param_rtl_time_margin.get(); + + } else { + time_estimate.valid = false; + } + + time_estimate.timestamp = hrt_absolute_time(); + return time_estimate; +} + +void RtlTimeEstimator::update() +{ + _vehicle_status_sub.update(); + _wind_sub.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 RtlTimeEstimator::addVertDistance(float alt) +{ + if (PX4_ISFINITE(alt)) { + _is_valid = true; + + _time_estimate += calcVertTimeEstimate(alt); + } +} + +void RtlTimeEstimator::addDistance(float hor_dist, const matrix::Vector2f &direction, float vert_dist) +{ + if (PX4_ISFINITE(hor_dist) && PX4_ISFINITE(vert_dist)) { + _is_valid = true; + + float hor_time_estimate{0.f}; + + if (hor_dist > FLT_EPSILON && PX4_ISFINITE(hor_dist)) { + hor_time_estimate = hor_dist / getCruiseGroundSpeed(direction.normalized()); + } + + float ver_time_estimate{calcVertTimeEstimate(vert_dist)}; + + _time_estimate += math::max(hor_time_estimate, ver_time_estimate); + + } +} + +void RtlTimeEstimator::addWait(float time_s) +{ + if (PX4_ISFINITE(time_s)) { + _is_valid = true; + + if (time_s > FLT_EPSILON) { + _time_estimate += time_s; + } + } +} + +void RtlTimeEstimator::addDescendMCLand(float alt) +{ + if (PX4_ISFINITE(alt)) { + _is_valid = true; + + if (alt < -FLT_EPSILON && PX4_ISFINITE(alt)) { + _time_estimate += -alt / getHoverLandSpeed(); + } + } +} + +float RtlTimeEstimator::getCruiseGroundSpeed(const matrix::Vector2f &direction_norm) +{ + float cruise_speed = getCruiseSpeed(); + + if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + matrix::Vector2f wind = get_wind(); + + const float wind_along_dir = wind.dot(direction_norm); + const float wind_across_dir = matrix::Vector2f(wind - direction_norm * wind_along_dir).norm(); + + // Note: use fminf so that we don't _rely_ on tailwind towards direction to make RTL more efficient + const float ground_speed = sqrtf(cruise_speed * cruise_speed - wind_across_dir * wind_across_dir) + fminf( + 0.f, wind_along_dir); + + cruise_speed = ground_speed; + } + + return cruise_speed; +} + +float RtlTimeEstimator::calcVertTimeEstimate(float alt) +{ + float vertical_rate{0.1f}; + float time_estimate{0.f}; + + if (alt > FLT_EPSILON) { + vertical_rate = getClimbRate(); + + } else { + vertical_rate = getDescendRate(); + } + + float abs_alt = fabsf(alt); + + if (abs_alt > FLT_EPSILON) { + time_estimate = abs_alt / vertical_rate; + } + + return time_estimate; +} + +float RtlTimeEstimator::getCruiseSpeed() +{ + float ret = 1e6f; + + if (_vehicle_status_sub.get().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 (_vehicle_status_sub.get().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 (_vehicle_status_sub.get().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 RtlTimeEstimator::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 RtlTimeEstimator::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 RtlTimeEstimator::getClimbRate() +{ + float ret = 1e6f; + + if (_vehicle_status_sub.get().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 (_vehicle_status_sub.get().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 RtlTimeEstimator::getDescendRate() +{ + float ret = 1e6f; + + if (_vehicle_status_sub.get().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 (_vehicle_status_sub.get().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; +} diff --git a/src/lib/rtl/rtl_time_estimator.h b/src/lib/rtl/rtl_time_estimator.h new file mode 100644 index 000000000000..db0f8c234ea3 --- /dev/null +++ b/src/lib/rtl/rtl_time_estimator.h @@ -0,0 +1,146 @@ +/**************************************************************************** + * + * Copyright (C) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file rtl_time_estimator.h + * + * Helper class to calculate the remaining time estimate to go to RTL landing point. + * + */ + +#ifndef RTL_TIME_ESTIMATOR_H_ +#define RTL_TIME_ESTIMATOR_H_ + +#include + +#include +#include +#include +#include +#include +#include + +#include + +using namespace time_literals; + +class Navigator; + +class RtlTimeEstimator : public ModuleParams +{ +public: + RtlTimeEstimator(); + ~RtlTimeEstimator() = default; + + void update(); + void reset() { _time_estimate = 0.f; _is_valid = false;}; + rtl_time_estimate_s getEstimate() const; + void addDistance(float hor_dist, const matrix::Vector2f &hor_direction, float vert_dist); + void addVertDistance(float alt); + void addWait(float time_s); + void addDescendMCLand(float alt); + +private: + /** + * @brief Get the Cruise Ground Speed + * + * @param direction_norm normalized direction in which to fly + * @return Ground speed in cruise mode [m/s]. + */ + float getCruiseGroundSpeed(const matrix::Vector2f &direction_norm); + + /** + * @brief Get time estimate of vertical distance + * + */ + float calcVertTimeEstimate(float alt); + + /** + * @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(); + + /** + * @brief Get the horizontal wind velocity + * + * @return horizontal wind velocity. + */ + matrix::Vector2f get_wind(); + + float _time_estimate; /**< Accumulated time estimate [s] */ + bool _is_valid{false}; /**< Checks if time estimate is valid */ + + DEFINE_PARAMETERS( + (ParamFloat) _param_rtl_time_factor, /**< Safety factory for safe time estimate */ + (ParamInt) _param_rtl_time_margin /**< Safety margin for safe time estimate */ + ) + + param_t _param_mpc_z_v_auto_up{PARAM_INVALID}; /**< MC climb velocity parameter */ + param_t _param_mpc_z_v_auto_dn{PARAM_INVALID}; /**< MC descend velocity parameter */ + param_t _param_mpc_land_speed{PARAM_INVALID}; /**< MC land descend speed parameter */ + param_t _param_fw_climb_rate{PARAM_INVALID}; /**< FW climb speed parameter */ + param_t _param_fw_sink_rate{PARAM_INVALID}; /**< FW descend speed parameter */ + + param_t _param_fw_airspeed_trim{PARAM_INVALID}; /**< FW cruise airspeed parameter */ + param_t _param_mpc_xy_cruise{PARAM_INVALID}; /**< MC horizontal cruise speed parameter */ + param_t _param_rover_cruise_speed{PARAM_INVALID}; /**< Rover cruise speed parameter */ + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; /**< Parameter update topic */ + uORB::SubscriptionData _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ + uORB::SubscriptionData _wind_sub{ORB_ID(wind)}; /**< wind topic */ +}; + +#endif /* RTL_TIME_ESTIMATOR_H_ */ diff --git a/src/lib/system_identification/signal_generator.hpp b/src/lib/system_identification/signal_generator.hpp new file mode 100644 index 000000000000..b61b0991cdef --- /dev/null +++ b/src/lib/system_identification/signal_generator.hpp @@ -0,0 +1,66 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file signal_generator.hpp + */ + +#pragma once + +namespace signal_generator +{ + +inline float getLinearSineSweep(float f_start, float f_end, float duration, float t) +{ + if (t > duration) { + return 0.f; + } + + const float w_start = f_start * M_TWOPI_F; + const float w_end = f_end * M_TWOPI_F; + + return sinf(w_start * t + 0.5f * (w_end - w_start) * t * t / duration); +} + +inline float getLogSineSweep(float f_start, float f_end, float duration, float t) +{ + if (t > duration) { + return 0.f; + } + + const float f_ratio = f_end / fmaxf(f_start, 0.1f); + + return sinf(M_TWOPI_F * f_start * duration * (powf(f_ratio, t / duration) - 1.f) / logf(f_ratio)); +} + +} /* namespace signal_generator */ diff --git a/src/lib/systemlib/mavlink_log.cpp b/src/lib/systemlib/mavlink_log.cpp index d31136f21530..94870af7ae12 100644 --- a/src/lib/systemlib/mavlink_log.cpp +++ b/src/lib/systemlib/mavlink_log.cpp @@ -73,6 +73,6 @@ __EXPORT void mavlink_vasprintf(int severity, orb_advert_t *mavlink_log_pub, con orb_publish(ORB_ID(mavlink_log), *mavlink_log_pub, &log_msg); } else { - *mavlink_log_pub = orb_advertise_queue(ORB_ID(mavlink_log), &log_msg, mavlink_log_s::ORB_QUEUE_LENGTH); + *mavlink_log_pub = orb_advertise(ORB_ID(mavlink_log), &log_msg); } } diff --git a/src/lib/version/version.h b/src/lib/version/version.h index d7d471417e67..451393f835ed 100644 --- a/src/lib/version/version.h +++ b/src/lib/version/version.h @@ -88,6 +88,16 @@ static inline int px4_board_hw_revision(void) return board_get_hw_revision(); } +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) +/** + * get the base board type + */ +static inline const char *px4_board_base_type(void) +{ + return board_get_hw_base_type_name(); +} +#endif + /** * get the build URI (used for crash logging) */ diff --git a/src/lib/world_magnetic_model/geo_magnetic_tables.hpp b/src/lib/world_magnetic_model/geo_magnetic_tables.hpp index 0340864bfbc3..3a1268ae400e 100644 --- a/src/lib/world_magnetic_model/geo_magnetic_tables.hpp +++ b/src/lib/world_magnetic_model/geo_magnetic_tables.hpp @@ -47,57 +47,57 @@ static constexpr int LON_DIM = 37; // Magnetic declination data in radians * 10^-4 // Model: WMM-2020, // Version: 0.5.1.11, -// Date: 2023.9315, +// Date: 2024.0684, 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 */ { 25952, 24207, 22462, 20716, 18971, 17226, 15480, 13735, 11990, 10244, 8499, 6754, 5008, 3263, 1518, -228, -1973, -3718, -5464, -7209, -8954,-10700,-12445,-14190,-15936,-17681,-19426,-21172,-22917,-24662,-26408,-28153,-29898, 31188, 29443, 27698, 25952, }, - /* LAT: -80 */ { 22515, 20388, 18451, 16680, 15041, 13504, 12041, 10631, 9256, 7907, 6575, 5257, 3949, 2644, 1336, 15, -1327, -2701, -4113, -5568, -7065, -8604,-10184,-11806,-13472,-15192,-16980,-18855,-20840,-22960,-25236,-27672,-30240, 29955, 27341, 24840, 22515, }, - /* LAT: -70 */ { 14987, 13586, 12457, 11491, 10619, 9784, 8940, 8050, 7096, 6077, 5006, 3914, 2831, 1778, 758, -252, -1290, -2400, -3606, -4912, -6299, -7734, -9185,-10630,-12058,-13476,-14907,-16397,-18027,-19952,-22493,-26319, 30594, 24093, 19618, 16857, 14987, }, // WARNING! black out zone - /* LAT: -60 */ { 8456, 8203, 7915, 7635, 7375, 7116, 6804, 6367, 5748, 4925, 3922, 2811, 1694, 672, -201, -953, -1681, -2509, -3523, -4733, -6074, -7452, -8776, -9988,-11051,-11947,-12661,-13154,-13326,-12878,-10764, -3393, 5042, 7739, 8486, 8600, 8456, }, // WARNING! black out zone - /* LAT: -50 */ { 5513, 5546, 5485, 5391, 5312, 5270, 5232, 5101, 4752, 4084, 3068, 1793, 455, -714, -1569, -2118, -2506, -2950, -3652, -4689, -5953, -7241, -8384, -9282, -9872,-10100, -9893, -9123, -7604, -5234, -2323, 426, 2537, 3963, 4837, 5311, 5513, }, - /* LAT: -40 */ { 3975, 4066, 4070, 4020, 3955, 3918, 3920, 3906, 3728, 3186, 2154, 705, -856, -2156, -2999, -3430, -3598, -3650, -3832, -4437, -5448, -6539, -7421, -7947, -8037, -7640, -6733, -5341, -3634, -1941, -488, 732, 1778, 2649, 3309, 3743, 3975, }, - /* LAT: -30 */ { 3001, 3086, 3112, 3091, 3028, 2945, 2882, 2846, 2717, 2231, 1182, -348, -1953, -3190, -3909, -4235, -4296, -4064, -3626, -3441, -3853, -4621, -5312, -5643, -5497, -4891, -3920, -2717, -1522, -590, 80, 678, 1305, 1916, 2432, 2799, 3001, }, - /* LAT: -20 */ { 2358, 2403, 2415, 2409, 2361, 2262, 2150, 2072, 1926, 1423, 351, -1150, -2624, -3666, -4175, -4270, -4047, -3468, -2599, -1824, -1589, -1972, -2624, -3079, -3098, -2724, -2082, -1271, -509, -32, 222, 521, 967, 1455, 1886, 2200, 2358, }, - /* LAT: -10 */ { 1964, 1957, 1930, 1919, 1885, 1795, 1679, 1584, 1398, 842, -233, -1618, -2883, -3691, -3931, -3679, -3089, -2300, -1456, -713, -268, -325, -807, -1309, -1510, -1410, -1093, -598, -111, 118, 157, 313, 690, 1140, 1544, 1840, 1964, }, - /* LAT: 0 */ { 1751, 1716, 1654, 1638, 1621, 1546, 1433, 1314, 1058, 434, -614, -1842, -2885, -3449, -3422, -2902, -2128, -1349, -703, -173, 233, 330, 37, -380, -628, -679, -581, -323, -40, 39, -33, 51, 399, 855, 1286, 1616, 1751, }, - /* LAT: 10 */ { 1611, 1618, 1570, 1581, 1601, 1546, 1416, 1223, 845, 127, -896, -1968, -2781, -3100, -2875, -2256, -1477, -771, -263, 113, 435, 575, 403, 80, -150, -258, -285, -210, -112, -162, -310, -288, 21, 487, 979, 1395, 1611, }, - /* LAT: 20 */ { 1419, 1567, 1624, 1712, 1795, 1769, 1607, 1299, 753, -101, -1141, -2084, -2666, -2758, -2417, -1807, -1096, -451, 2, 307, 564, 704, 606, 362, 165, 45, -53, -126, -213, -409, -652, -713, -473, -15, 534, 1058, 1419, }, - /* LAT: 30 */ { 1106, 1471, 1730, 1953, 2111, 2119, 1926, 1500, 772, -253, -1362, -2226, -2627, -2552, -2146, -1564, -908, -293, 164, 465, 694, 835, 809, 659, 511, 384, 218, -4, -296, -675, -1047, -1205, -1040, -607, -27, 584, 1106, }, - /* LAT: 40 */ { 734, 1319, 1814, 2208, 2458, 2501, 2285, 1751, 841, -378, -1603, -2454, -2762, -2603, -2154, -1560, -906, -279, 227, 587, 854, 1048, 1135, 1122, 1046, 894, 615, 190, -360, -971, -1491, -1730, -1608, -1188, -590, 79, 734, }, - /* LAT: 50 */ { 428, 1172, 1855, 2416, 2787, 2897, 2671, 2021, 888, -593, -1996, -2895, -3182, -2987, -2496, -1850, -1140, -445, 168, 670, 1084, 1434, 1708, 1874, 1889, 1695, 1235, 509, -398, -1302, -1974, -2252, -2121, -1681, -1052, -329, 428, }, - /* LAT: 60 */ { 208, 1056, 1863, 2564, 3077, 3299, 3096, 2301, 812, -1114, -2805, -3776, -4028, -3771, -3202, -2459, -1636, -799, 6, 754, 1443, 2069, 2607, 2996, 3146, 2945, 2285, 1144, -285, -1601, -2457, -2755, -2587, -2099, -1417, -630, 208, }, - /* LAT: 70 */ { -70, 858, 1754, 2556, 3179, 3487, 3262, 2180, 33, -2597, -4529, -5370, -5400, -4934, -4177, -3252, -2235, -1175, -105, 951, 1973, 2935, 3799, 4500, 4937, 4952, 4314, 2801, 574, -1537, -2821, -3256, -3098, -2576, -1840, -985, -70, }, // WARNING! black out zone - /* LAT: 80 */ { -920, -2, 846, 1538, 1932, 1791, 724, -1605, -4558, -6653, -7479, -7425, -6852, -5974, -4911, -3732, -2481, -1186, 131, 1453, 2764, 4044, 5271, 6410, 7409, 8171, 8512, 8044, 6013, 2014, -1698, -3391, -3721, -3373, -2689, -1839, -920, }, // WARNING! black out zone - /* LAT: 90 */ { -29309,-27563,-25818,-24073,-22327,-20582,-18837,-17091,-15346,-13601,-11855,-10110, -8365, -6619, -4874, -3129, -1383, 362, 2107, 3853, 5598, 7343, 9089, 10834, 12579, 14325, 16070, 17815, 19561, 21306, 23051, 24797, 26542, 28287, 30033,-31054,-29309, }, // WARNING! black out zone + /* LAT: -90 */ { 25949, 24204, 22458, 20713, 18968, 17222, 15477, 13732, 11986, 10241, 8496, 6750, 5005, 3260, 1514, -231, -1976, -3722, -5467, -7212, -8958,-10703,-12448,-14194,-15939,-17684,-19430,-21175,-22920,-24666,-26411,-28156,-29902, 31185, 29440, 27694, 25949, }, + /* LAT: -80 */ { 22511, 20385, 18448, 16677, 15039, 13502, 12039, 10628, 9254, 7905, 6573, 5255, 3947, 2642, 1334, 13, -1329, -2704, -4116, -5571, -7068, -8608,-10188,-11810,-13476,-15197,-16985,-18860,-20845,-22966,-25242,-27677,-30245, 29950, 27336, 24836, 22511, }, + /* LAT: -70 */ { 14988, 13587, 12457, 11491, 10618, 9783, 8939, 8049, 7095, 6075, 5005, 3913, 2830, 1777, 757, -252, -1291, -2401, -3608, -4915, -6302, -7737, -9189,-10634,-12063,-13481,-14912,-16402,-18033,-19960,-22502,-26331, 30582, 24088, 19617, 16858, 14988, }, // WARNING! black out zone + /* LAT: -60 */ { 8460, 8207, 7918, 7636, 7376, 7117, 6803, 6366, 5747, 4924, 3920, 2809, 1692, 671, -201, -952, -1680, -2508, -3524, -4735, -6077, -7455, -8781, -9992,-11055,-11951,-12664,-13158,-13330,-12882,-10766, -3382, 5057, 7748, 8493, 8605, 8460, }, // WARNING! black out zone + /* LAT: -50 */ { 5516, 5549, 5488, 5392, 5313, 5271, 5232, 5100, 4751, 4082, 3066, 1790, 453, -715, -1568, -2116, -2503, -2947, -3651, -4691, -5957, -7246, -8389, -9286, -9875,-10102, -9893, -9123, -7602, -5231, -2320, 430, 2541, 3966, 4841, 5314, 5516, }, + /* LAT: -40 */ { 3977, 4068, 4072, 4021, 3956, 3918, 3919, 3905, 3727, 3185, 2151, 702, -859, -2158, -2999, -3428, -3594, -3645, -3829, -4438, -5452, -6544, -7426, -7951, -8039, -7639, -6731, -5338, -3632, -1939, -487, 732, 1779, 2650, 3311, 3745, 3977, }, + /* LAT: -30 */ { 3003, 3088, 3113, 3092, 3028, 2945, 2881, 2845, 2716, 2229, 1178, -353, -1958, -3193, -3909, -4234, -4292, -4059, -3621, -3439, -3855, -4626, -5316, -5645, -5497, -4889, -3917, -2714, -1520, -590, 79, 677, 1305, 1916, 2433, 2801, 3003, }, + /* LAT: -20 */ { 2360, 2404, 2416, 2410, 2361, 2262, 2149, 2071, 1924, 1420, 347, -1155, -2629, -3669, -4175, -4268, -4043, -3462, -2592, -1820, -1589, -1975, -2627, -3080, -3097, -2722, -2080, -1269, -508, -33, 221, 519, 966, 1455, 1887, 2202, 2360, }, + /* LAT: -10 */ { 1966, 1959, 1931, 1919, 1885, 1794, 1677, 1582, 1396, 839, -238, -1623, -2888, -3693, -3930, -3676, -3084, -2295, -1450, -709, -266, -325, -809, -1310, -1509, -1409, -1091, -597, -111, 118, 156, 311, 689, 1139, 1545, 1841, 1966, }, + /* LAT: 0 */ { 1752, 1717, 1655, 1639, 1621, 1545, 1432, 1311, 1055, 430, -618, -1847, -2888, -3450, -3420, -2898, -2123, -1344, -699, -169, 235, 331, 37, -380, -627, -679, -580, -323, -41, 38, -35, 49, 397, 854, 1287, 1617, 1752, }, + /* LAT: 10 */ { 1612, 1619, 1571, 1581, 1601, 1545, 1414, 1220, 842, 124, -899, -1971, -2783, -3100, -2872, -2252, -1473, -767, -260, 116, 438, 577, 403, 80, -149, -257, -285, -210, -113, -163, -313, -290, 19, 487, 979, 1396, 1612, }, + /* LAT: 20 */ { 1419, 1567, 1624, 1712, 1794, 1768, 1605, 1296, 751, -104, -1144, -2086, -2666, -2757, -2413, -1803, -1092, -447, 5, 310, 566, 706, 607, 362, 166, 46, -53, -127, -214, -411, -655, -715, -474, -16, 534, 1059, 1419, }, + /* LAT: 30 */ { 1105, 1471, 1730, 1952, 2110, 2117, 1924, 1498, 769, -255, -1364, -2227, -2625, -2550, -2143, -1560, -904, -289, 167, 468, 696, 837, 810, 660, 512, 385, 219, -5, -297, -677, -1049, -1207, -1042, -608, -27, 584, 1105, }, + /* LAT: 40 */ { 732, 1317, 1812, 2206, 2456, 2499, 2283, 1748, 839, -380, -1603, -2452, -2759, -2599, -2149, -1556, -902, -275, 231, 591, 857, 1050, 1137, 1123, 1047, 895, 615, 189, -362, -974, -1493, -1732, -1610, -1189, -591, 77, 732, }, + /* LAT: 50 */ { 425, 1168, 1851, 2413, 2785, 2895, 2669, 2019, 887, -593, -1994, -2892, -3177, -2982, -2491, -1845, -1135, -440, 172, 674, 1088, 1437, 1711, 1876, 1891, 1695, 1235, 507, -401, -1306, -1977, -2253, -2123, -1682, -1054, -331, 425, }, + /* LAT: 60 */ { 203, 1050, 1857, 2558, 3072, 3295, 3093, 2299, 813, -1110, -2799, -3769, -4020, -3763, -3195, -2453, -1630, -793, 11, 759, 1447, 2073, 2610, 2999, 3148, 2946, 2284, 1141, -289, -1606, -2460, -2757, -2588, -2101, -1421, -635, 203, }, + /* LAT: 70 */ { -80, 847, 1743, 2546, 3170, 3479, 3255, 2178, 39, -2583, -4513, -5355, -5387, -4922, -4167, -3243, -2226, -1167, -98, 958, 1979, 2941, 3805, 4505, 4941, 4955, 4314, 2796, 566, -1545, -2828, -3261, -3103, -2582, -1848, -994, -80, }, // WARNING! black out zone + /* LAT: 80 */ { -944, -26, 822, 1513, 1909, 1772, 716, -1590, -4521, -6612, -7444, -7396, -6828, -5954, -4893, -3716, -2466, -1173, 144, 1465, 2775, 4055, 5282, 6422, 7420, 8182, 8523, 8052, 6011, 1989, -1732, -3421, -3747, -3398, -2713, -1863, -944, }, // WARNING! black out zone + /* LAT: 90 */ { -29255,-27510,-25765,-24019,-22274,-20529,-18783,-17038,-15293,-13547,-11802,-10057, -8311, -6566, -4821, -3075, -1330, 415, 2161, 3906, 5651, 7397, 9142, 10887, 12633, 14378, 16123, 17869, 19614, 21359, 23105, 24850, 26595, 28341, 30086,-31001,-29255, }, // WARNING! black out zone }; -static constexpr float WMM_DECLINATION_MIN_RAD = -3.105; // latitude: 90, longitude: 170 -static constexpr float WMM_DECLINATION_MAX_RAD = 3.119; // latitude: -90, longitude: 150 +static constexpr float WMM_DECLINATION_MIN_RAD = -3.100; // latitude: 90, longitude: 170 +static constexpr float WMM_DECLINATION_MAX_RAD = 3.118; // latitude: -90, longitude: 150 // Magnetic inclination data in radians * 10^-4 // Model: WMM-2020, // Version: 0.5.1.11, -// Date: 2023.9315, +// Date: 2024.0684, 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 */ { -12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563, }, - /* LAT: -80 */ { -13645,-13511,-13350,-13170,-12977,-12775,-12572,-12372,-12182,-12006,-11850,-11716,-11606,-11520,-11456,-11414,-11394,-11397,-11424,-11478,-11562,-11677,-11822,-11996,-12195,-12412,-12642,-12875,-13102,-13315,-13501,-13650,-13753,-13803,-13799,-13744,-13645, }, - /* LAT: -70 */ { -14092,-13773,-13454,-13131,-12800,-12457,-12103,-11746,-11405,-11098,-10847,-10665,-10554,-10501,-10487,-10488,-10493,-10500,-10522,-10577,-10686,-10862,-11110,-11427,-11802,-12221,-12670,-13135,-13601,-14053,-14469,-14811,-14996,-14938,-14706,-14408,-14092, }, // WARNING! black out zone - /* LAT: -60 */ { -13510,-13156,-12817,-12484,-12141,-11769,-11354,-10900,-10434,-10007, -9680, -9509, -9510, -9649, -9852,-10040,-10158,-10194,-10176,-10159,-10207,-10370,-10664,-11078,-11581,-12139,-12726,-13323,-13912,-14473,-14971,-15259,-15076,-14688,-14280,-13885,-13510, }, // WARNING! black out zone - /* LAT: -50 */ { -12492,-12149,-11817,-11494,-11170,-10824,-10425, -9955, -9427, -8909, -8525, -8408, -8618, -9083, -9646,-10150,-10498,-10648,-10606,-10446,-10304,-10316,-10548,-10973,-11521,-12116,-12702,-13239,-13683,-13979,-14085,-14011,-13807,-13523,-13193,-12844,-12492, }, - /* LAT: -40 */ { -11239,-10888,-10538,-10192, -9853, -9515, -9156, -8732, -8213, -7652, -7234, -7207, -7682, -8513, -9433,-10255,-10903,-11323,-11443,-11258,-10908,-10643,-10651,-10945,-11415,-11920,-12362,-12678,-12833,-12837,-12748,-12612,-12435,-12204,-11917,-11588,-11239, }, - /* LAT: -30 */ { -9603, -9219, -8835, -8443, -8051, -7678, -7324, -6937, -6425, -5817, -5376, -5482, -6276, -7490, -8739, -9825,-10728,-11423,-11800,-11766,-11378,-10870,-10549,-10564,-10823,-11145,-11396,-11502,-11435,-11259,-11088,-10959,-10818,-10613,-10332, -9985, -9603, }, - /* LAT: -20 */ { -7374, -6924, -6500, -6069, -5625, -5197, -4811, -4404, -3842, -3160, -2727, -3020, -4165, -5794, -7417, -8770, -9817,-10574,-10990,-11002,-10620, -9997, -9445, -9213, -9270, -9434, -9570, -9581, -9405, -9134, -8946, -8869, -8774, -8571, -8255, -7839, -7374, }, - /* LAT: -10 */ { -4419, -3870, -3407, -2966, -2508, -2059, -1651, -1206, -588, 108, 450, -14, -1381, -3318, -5286, -6868, -7936, -8546, -8791, -8710, -8282, -7584, -6922, -6587, -6554, -6648, -6761, -6780, -6589, -6295, -6150, -6175, -6150, -5941, -5557, -5024, -4419, }, - /* LAT: 0 */ { -912, -273, 203, 610, 1030, 1446, 1830, 2261, 2833, 3399, 3581, 3053, 1722, -210, -2248, -3868, -4842, -5244, -5285, -5101, -4643, -3912, -3207, -2845, -2788, -2855, -2976, -3047, -2916, -2684, -2641, -2805, -2888, -2711, -2286, -1648, -912, }, - /* LAT: 10 */ { 2555, 3196, 3640, 3985, 4342, 4708, 5054, 5429, 5868, 6226, 6246, 5733, 4625, 3041, 1354, 4, -770, -993, -886, -640, -215, 436, 1069, 1400, 1463, 1425, 1328, 1237, 1278, 1377, 1290, 1003, 791, 854, 1207, 1817, 2555, }, - /* LAT: 20 */ { 5412, 5950, 6336, 6635, 6949, 7291, 7629, 7968, 8290, 8481, 8381, 7903, 7051, 5946, 4824, 3932, 3424, 3324, 3486, 3737, 4076, 4549, 5010, 5263, 5322, 5311, 5267, 5208, 5193, 5170, 4986, 4639, 4327, 4231, 4404, 4832, 5412, }, - /* LAT: 30 */ { 7567, 7945, 8266, 8550, 8859, 9205, 9559, 9894, 10163, 10268, 10114, 9684, 9048, 8335, 7679, 7177, 6897, 6868, 7019, 7236, 7487, 7791, 8081, 8254, 8314, 8332, 8338, 8328, 8302, 8214, 7985, 7624, 7260, 7037, 7021, 7219, 7567, }, - /* LAT: 40 */ { 9266, 9487, 9744, 10030, 10356, 10715, 11080, 11414, 11661, 11737, 11582, 11218, 10744, 10273, 9881, 9603, 9461, 9464, 9581, 9747, 9926, 10113, 10286, 10411, 10490, 10554, 10612, 10646, 10626, 10511, 10264, 9909, 9538, 9251, 9109, 9122, 9266, }, - /* LAT: 50 */ { 10802, 10922, 11123, 11392, 11713, 12065, 12417, 12730, 12948, 13004, 12863, 12565, 12202, 11859, 11587, 11404, 11315, 11317, 11387, 11494, 11610, 11729, 11846, 11962, 12082, 12207, 12322, 12394, 12382, 12252, 12002, 11672, 11331, 11046, 10857, 10776, 10802, }, - /* LAT: 60 */ { 12320, 12391, 12539, 12754, 13022, 13320, 13620, 13884, 14059, 14086, 13951, 13704, 13417, 13148, 12929, 12776, 12688, 12659, 12678, 12728, 12798, 12885, 12992, 13126, 13286, 13464, 13628, 13736, 13738, 13614, 13387, 13107, 12829, 12593, 12423, 12331, 12320, }, - /* LAT: 70 */ { 13757, 13797, 13889, 14028, 14205, 14406, 14612, 14792, 14897, 14881, 14748, 14550, 14334, 14132, 13960, 13826, 13733, 13680, 13662, 13676, 13720, 13793, 13898, 14036, 14204, 14392, 14576, 14715, 14758, 14683, 14520, 14321, 14127, 13962, 13842, 13772, 13757, }, // WARNING! black out zone - /* LAT: 80 */ { 14991, 15002, 15037, 15094, 15168, 15250, 15327, 15373, 15360, 15288, 15180, 15059, 14938, 14825, 14727, 14645, 14584, 14544, 14525, 14530, 14557, 14607, 14679, 14773, 14885, 15013, 15149, 15281, 15389, 15431, 15388, 15298, 15201, 15115, 15049, 15007, 14991, }, // WARNING! black out zone + /* LAT: -90 */ { -12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562, }, + /* LAT: -80 */ { -13644,-13510,-13349,-13169,-12975,-12774,-12571,-12371,-12181,-12005,-11849,-11715,-11605,-11519,-11455,-11414,-11394,-11396,-11423,-11477,-11561,-11676,-11821,-11995,-12194,-12412,-12641,-12874,-13102,-13314,-13500,-13649,-13752,-13802,-13798,-13743,-13644, }, + /* LAT: -70 */ { -14091,-13772,-13453,-13130,-12799,-12455,-12102,-11746,-11404,-11097,-10846,-10664,-10553,-10501,-10487,-10488,-10492,-10499,-10521,-10576,-10685,-10861,-11110,-11427,-11802,-12221,-12670,-13135,-13601,-14053,-14469,-14811,-14995,-14937,-14705,-14407,-14091, }, // WARNING! black out zone + /* LAT: -60 */ { -13509,-13155,-12816,-12483,-12140,-11768,-11353,-10900,-10434,-10006, -9680, -9510, -9511, -9650, -9853,-10040,-10158,-10193,-10174,-10157,-10205,-10369,-10664,-11078,-11581,-12140,-12727,-13324,-13913,-14474,-14971,-15259,-15076,-14687,-14279,-13884,-13509, }, // WARNING! black out zone + /* LAT: -50 */ { -12492,-12148,-11816,-11493,-11170,-10823,-10425, -9954, -9426, -8909, -8526, -8410, -8620, -9086, -9648,-10152,-10499,-10648,-10604,-10443,-10301,-10315,-10547,-10974,-11522,-12117,-12703,-13240,-13683,-13979,-14086,-14012,-13807,-13523,-13193,-12844,-12492, }, + /* LAT: -40 */ { -11239,-10888,-10538,-10191, -9852, -9514, -9155, -8732, -8213, -7652, -7235, -7210, -7686, -8518, -9438,-10259,-10906,-11324,-11443,-11256,-10906,-10641,-10650,-10946,-11416,-11922,-12363,-12679,-12833,-12837,-12748,-12613,-12435,-12204,-11917,-11588,-11239, }, + /* LAT: -30 */ { -9603, -9218, -8834, -8442, -8050, -7677, -7323, -6936, -6425, -5817, -5378, -5486, -6282, -7498, -8746, -9831,-10733,-11426,-11801,-11765,-11376,-10868,-10548,-10564,-10824,-11146,-11397,-11501,-11434,-11258,-11088,-10959,-10818,-10614,-10333, -9985, -9603, }, + /* LAT: -20 */ { -7374, -6924, -6499, -6068, -5624, -5196, -4810, -4404, -3842, -3160, -2729, -3025, -4173, -5803, -7426, -8778, -9823,-10578,-10993,-11002,-10618, -9994, -9442, -9212, -9270, -9434, -9570, -9580, -9403, -9133, -8945, -8869, -8775, -8573, -8256, -7840, -7374, }, + /* LAT: -10 */ { -4420, -3869, -3405, -2963, -2506, -2057, -1649, -1205, -588, 107, 447, -20, -1391, -3330, -5297, -6877, -7942, -8550, -8793, -8710, -8280, -7581, -6919, -6585, -6553, -6646, -6760, -6778, -6587, -6293, -6148, -6175, -6151, -5943, -5559, -5026, -4420, }, + /* LAT: 0 */ { -913, -272, 205, 613, 1033, 1448, 1832, 2262, 2833, 3397, 3578, 3047, 1712, -222, -2259, -3876, -4847, -5247, -5286, -5101, -4641, -3909, -3204, -2843, -2786, -2852, -2974, -3044, -2913, -2681, -2639, -2805, -2890, -2713, -2288, -1650, -913, }, + /* LAT: 10 */ { 2555, 3197, 3642, 3988, 4344, 4710, 5056, 5430, 5867, 6224, 6243, 5728, 4617, 3031, 1345, -3, -774, -994, -886, -639, -213, 438, 1072, 1402, 1466, 1428, 1331, 1240, 1281, 1380, 1291, 1002, 789, 851, 1205, 1815, 2555, }, + /* LAT: 20 */ { 5412, 5951, 6338, 6637, 6951, 7293, 7631, 7968, 8290, 8479, 8378, 7898, 7045, 5939, 4818, 3927, 3420, 3323, 3486, 3737, 4077, 4551, 5012, 5264, 5324, 5314, 5270, 5211, 5196, 5172, 4987, 4639, 4326, 4229, 4402, 4831, 5412, }, + /* LAT: 30 */ { 7567, 7945, 8266, 8551, 8860, 9206, 9559, 9893, 10162, 10266, 10112, 9680, 9044, 8331, 7675, 7174, 6895, 6867, 7019, 7236, 7488, 7793, 8083, 8256, 8315, 8334, 8340, 8330, 8304, 8215, 7986, 7624, 7260, 7036, 7020, 7218, 7567, }, + /* LAT: 40 */ { 9266, 9488, 9744, 10030, 10356, 10715, 11080, 11414, 11660, 11735, 11580, 11216, 10741, 10270, 9878, 9601, 9460, 9464, 9581, 9747, 9926, 10114, 10287, 10412, 10492, 10556, 10614, 10648, 10628, 10512, 10265, 9909, 9538, 9251, 9108, 9122, 9266, }, + /* LAT: 50 */ { 10802, 10922, 11123, 11391, 11713, 12065, 12416, 12729, 12947, 13002, 12861, 12563, 12200, 11857, 11585, 11403, 11315, 11317, 11388, 11494, 11611, 11729, 11847, 11964, 12083, 12208, 12324, 12396, 12383, 12253, 12003, 11672, 11331, 11046, 10857, 10777, 10802, }, + /* LAT: 60 */ { 12320, 12390, 12538, 12754, 13021, 13319, 13619, 13883, 14057, 14084, 13950, 13702, 13416, 13147, 12929, 12775, 12687, 12659, 12678, 12728, 12798, 12885, 12993, 13127, 13288, 13465, 13630, 13737, 13739, 13614, 13387, 13107, 12829, 12593, 12423, 12331, 12320, }, + /* LAT: 70 */ { 13757, 13796, 13888, 14027, 14204, 14404, 14610, 14790, 14895, 14879, 14747, 14549, 14334, 14132, 13960, 13826, 13734, 13680, 13663, 13677, 13720, 13794, 13899, 14037, 14205, 14393, 14577, 14716, 14759, 14683, 14520, 14321, 14127, 13963, 13842, 13772, 13757, }, // WARNING! black out zone + /* LAT: 80 */ { 14991, 15001, 15036, 15092, 15166, 15249, 15325, 15371, 15359, 15287, 15180, 15059, 14938, 14826, 14727, 14646, 14584, 14544, 14526, 14531, 14558, 14608, 14680, 14774, 14886, 15014, 15150, 15283, 15390, 15432, 15389, 15299, 15201, 15115, 15049, 15007, 14991, }, // WARNING! black out zone /* LAT: 90 */ { 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, }, // WARNING! black out zone }; static constexpr float WMM_INCLINATION_MIN_RAD = -1.526; // latitude: -60, longitude: 130 @@ -107,27 +107,27 @@ static constexpr float WMM_INCLINATION_MAX_RAD = 1.543; // latitude: 80, longitu // Magnetic strength data in milli-Gauss * 10 // Model: WMM-2020, // Version: 0.5.1.11, -// Date: 2023.9315, +// Date: 2024.0684, static constexpr const int16_t strength_table[19][37] { // LONGITUDE: -180, -170, -160, -150, -140, -130, -120, -110, -100, -90, -80, -70, -60, -50, -40, -30, -20, -10, 0, 10, 20, 30, 40, 50, 60, 70, 80, 90, 100, 110, 120, 130, 140, 150, 160, 170, 180, - /* LAT: -90 */ { 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, }, - /* LAT: -80 */ { 6050, 5985, 5906, 5813, 5710, 5598, 5479, 5357, 5235, 5115, 5001, 4896, 4803, 4723, 4661, 4616, 4592, 4591, 4613, 4659, 4731, 4825, 4940, 5073, 5217, 5367, 5517, 5660, 5791, 5905, 5997, 6066, 6110, 6128, 6123, 6097, 6050, }, - /* LAT: -70 */ { 6294, 6160, 6008, 5842, 5663, 5470, 5264, 5049, 4831, 4616, 4414, 4232, 4075, 3945, 3841, 3765, 3718, 3707, 3736, 3813, 3943, 4124, 4354, 4623, 4919, 5227, 5531, 5815, 6064, 6268, 6418, 6513, 6553, 6544, 6493, 6408, 6294, }, - /* LAT: -60 */ { 6180, 5986, 5783, 5573, 5353, 5118, 4861, 4583, 4291, 4002, 3736, 3509, 3331, 3197, 3099, 3025, 2975, 2955, 2982, 3074, 3245, 3499, 3828, 4216, 4640, 5074, 5496, 5879, 6203, 6451, 6613, 6690, 6691, 6626, 6511, 6358, 6180, }, - /* LAT: -50 */ { 5839, 5607, 5373, 5141, 4908, 4662, 4391, 4088, 3759, 3428, 3127, 2889, 2729, 2639, 2591, 2556, 2521, 2493, 2499, 2573, 2750, 3045, 3445, 3919, 4427, 4933, 5407, 5825, 6163, 6403, 6540, 6579, 6535, 6423, 6260, 6060, 5839, }, - /* LAT: -40 */ { 5389, 5142, 4896, 4655, 4420, 4182, 3927, 3642, 3328, 3003, 2705, 2484, 2369, 2345, 2364, 2383, 2386, 2372, 2358, 2388, 2523, 2805, 3229, 3747, 4295, 4818, 5282, 5668, 5959, 6146, 6233, 6234, 6163, 6031, 5849, 5629, 5389, }, - /* LAT: -30 */ { 4876, 4634, 4393, 4158, 3931, 3711, 3492, 3261, 3005, 2729, 2472, 2291, 2224, 2251, 2317, 2388, 2451, 2499, 2518, 2529, 2600, 2807, 3177, 3670, 4203, 4698, 5113, 5426, 5627, 5726, 5752, 5724, 5644, 5511, 5331, 5113, 4876, }, - /* LAT: -20 */ { 4320, 4105, 3895, 3690, 3493, 3310, 3141, 2977, 2799, 2600, 2409, 2277, 2240, 2286, 2376, 2487, 2614, 2740, 2824, 2857, 2883, 2985, 3233, 3623, 4076, 4501, 4846, 5077, 5179, 5186, 5156, 5109, 5027, 4899, 4732, 4534, 4320, }, - /* LAT: -10 */ { 3789, 3627, 3473, 3326, 3190, 3070, 2966, 2873, 2774, 2658, 2536, 2438, 2396, 2424, 2513, 2643, 2799, 2955, 3075, 3134, 3148, 3178, 3307, 3564, 3893, 4215, 4477, 4636, 4669, 4616, 4548, 4485, 4397, 4273, 4123, 3958, 3789, }, - /* LAT: 0 */ { 3412, 3318, 3233, 3159, 3103, 3065, 3038, 3017, 2991, 2942, 2862, 2769, 2691, 2664, 2710, 2814, 2947, 3081, 3194, 3267, 3297, 3320, 3398, 3560, 3771, 3985, 4164, 4268, 4271, 4203, 4115, 4023, 3912, 3780, 3646, 3521, 3412, }, - /* LAT: 10 */ { 3282, 3250, 3229, 3224, 3248, 3295, 3349, 3400, 3433, 3422, 3353, 3239, 3114, 3023, 3000, 3044, 3126, 3225, 3325, 3409, 3472, 3535, 3625, 3746, 3883, 4023, 4142, 4211, 4212, 4149, 4038, 3894, 3732, 3573, 3437, 3339, 3282, }, - /* LAT: 20 */ { 3399, 3400, 3425, 3478, 3568, 3688, 3816, 3931, 4010, 4022, 3948, 3807, 3641, 3505, 3433, 3423, 3460, 3535, 3632, 3729, 3821, 3920, 4032, 4145, 4255, 4368, 4471, 4536, 4545, 4485, 4346, 4143, 3915, 3705, 3541, 3439, 3399, }, - /* LAT: 30 */ { 3722, 3726, 3779, 3876, 4018, 4188, 4362, 4517, 4624, 4650, 4577, 4422, 4236, 4074, 3972, 3929, 3935, 3988, 4076, 4175, 4274, 4381, 4498, 4616, 4736, 4865, 4986, 5072, 5097, 5037, 4879, 4638, 4363, 4108, 3908, 3779, 3722, }, - /* LAT: 40 */ { 4222, 4217, 4279, 4400, 4565, 4751, 4933, 5089, 5194, 5220, 5152, 5005, 4821, 4649, 4524, 4451, 4427, 4452, 4514, 4595, 4682, 4780, 4896, 5029, 5181, 5344, 5496, 5605, 5642, 5585, 5428, 5189, 4915, 4656, 4445, 4298, 4222, }, - /* LAT: 50 */ { 4832, 4821, 4873, 4980, 5124, 5282, 5432, 5554, 5628, 5638, 5576, 5451, 5290, 5129, 4993, 4898, 4845, 4835, 4861, 4913, 4983, 5075, 5195, 5345, 5521, 5706, 5872, 5988, 6030, 5983, 5851, 5655, 5432, 5217, 5037, 4906, 4832, }, - /* LAT: 60 */ { 5393, 5376, 5400, 5460, 5544, 5638, 5725, 5793, 5828, 5821, 5768, 5676, 5557, 5430, 5312, 5218, 5152, 5120, 5119, 5147, 5205, 5292, 5411, 5558, 5725, 5893, 6039, 6141, 6183, 6159, 6076, 5951, 5807, 5665, 5542, 5449, 5393, }, - /* LAT: 70 */ { 5726, 5703, 5698, 5708, 5728, 5754, 5778, 5794, 5795, 5778, 5740, 5685, 5616, 5541, 5468, 5405, 5358, 5331, 5327, 5347, 5392, 5462, 5554, 5663, 5780, 5894, 5992, 6064, 6102, 6105, 6076, 6023, 5957, 5886, 5820, 5766, 5726, }, - /* LAT: 80 */ { 5790, 5772, 5756, 5743, 5733, 5723, 5713, 5702, 5688, 5670, 5649, 5625, 5599, 5572, 5548, 5529, 5515, 5511, 5516, 5532, 5558, 5593, 5636, 5684, 5733, 5781, 5823, 5858, 5881, 5894, 5896, 5889, 5874, 5855, 5833, 5811, 5790, }, + /* LAT: -90 */ { 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, }, + /* LAT: -80 */ { 6049, 5985, 5905, 5812, 5709, 5596, 5478, 5356, 5234, 5114, 5000, 4895, 4802, 4722, 4660, 4615, 4591, 4590, 4612, 4659, 4730, 4825, 4940, 5072, 5216, 5367, 5517, 5660, 5791, 5905, 5997, 6065, 6109, 6128, 6123, 6096, 6049, }, + /* LAT: -70 */ { 6294, 6159, 6007, 5841, 5661, 5468, 5263, 5048, 4829, 4615, 4413, 4231, 4074, 3943, 3840, 3764, 3718, 3706, 3735, 3813, 3942, 4124, 4354, 4623, 4919, 5227, 5531, 5815, 6064, 6268, 6418, 6512, 6553, 6544, 6493, 6407, 6294, }, + /* LAT: -60 */ { 6179, 5985, 5782, 5572, 5352, 5116, 4860, 4581, 4289, 4000, 3734, 3508, 3330, 3196, 3098, 3025, 2974, 2954, 2981, 3074, 3245, 3499, 3829, 4217, 4640, 5075, 5496, 5880, 6204, 6451, 6613, 6690, 6691, 6626, 6511, 6358, 6179, }, + /* LAT: -50 */ { 5838, 5606, 5373, 5140, 4907, 4661, 4390, 4087, 3758, 3427, 3126, 2888, 2728, 2638, 2590, 2555, 2520, 2492, 2498, 2572, 2750, 3045, 3445, 3920, 4428, 4934, 5408, 5826, 6163, 6403, 6540, 6579, 6535, 6423, 6260, 6060, 5838, }, + /* LAT: -40 */ { 5389, 5142, 4895, 4654, 4419, 4181, 3925, 3641, 3327, 3001, 2704, 2483, 2369, 2344, 2363, 2383, 2385, 2371, 2357, 2387, 2523, 2806, 3230, 3748, 4296, 4819, 5283, 5669, 5960, 6146, 6233, 6234, 6164, 6031, 5849, 5629, 5389, }, + /* LAT: -30 */ { 4876, 4633, 4393, 4157, 3930, 3711, 3491, 3260, 3004, 2728, 2471, 2291, 2224, 2250, 2317, 2387, 2451, 2498, 2517, 2528, 2600, 2807, 3178, 3672, 4204, 4700, 5114, 5427, 5628, 5727, 5752, 5725, 5645, 5511, 5331, 5113, 4876, }, + /* LAT: -20 */ { 4320, 4105, 3895, 3689, 3492, 3310, 3141, 2977, 2798, 2599, 2408, 2277, 2239, 2286, 2376, 2487, 2614, 2739, 2823, 2856, 2882, 2985, 3234, 3624, 4077, 4503, 4847, 5077, 5180, 5186, 5156, 5109, 5027, 4899, 4732, 4534, 4320, }, + /* LAT: -10 */ { 3789, 3627, 3472, 3325, 3190, 3069, 2965, 2872, 2773, 2657, 2535, 2437, 2396, 2424, 2514, 2644, 2799, 2955, 3075, 3134, 3147, 3177, 3307, 3565, 3894, 4216, 4478, 4637, 4669, 4616, 4548, 4485, 4398, 4273, 4123, 3959, 3789, }, + /* LAT: 0 */ { 3412, 3317, 3232, 3159, 3103, 3064, 3037, 3017, 2990, 2941, 2861, 2768, 2690, 2664, 2710, 2815, 2947, 3082, 3194, 3267, 3297, 3320, 3399, 3560, 3772, 3986, 4165, 4268, 4272, 4203, 4115, 4023, 3912, 3781, 3646, 3521, 3412, }, + /* LAT: 10 */ { 3282, 3250, 3229, 3224, 3248, 3294, 3348, 3399, 3432, 3421, 3352, 3238, 3113, 3022, 3000, 3044, 3126, 3225, 3325, 3409, 3473, 3536, 3626, 3746, 3884, 4024, 4143, 4212, 4212, 4149, 4038, 3895, 3733, 3573, 3437, 3339, 3282, }, + /* LAT: 20 */ { 3399, 3400, 3425, 3477, 3568, 3688, 3815, 3930, 4009, 4020, 3946, 3805, 3640, 3504, 3432, 3423, 3460, 3535, 3632, 3730, 3821, 3921, 4033, 4145, 4256, 4369, 4472, 4537, 4546, 4485, 4347, 4143, 3915, 3705, 3541, 3439, 3399, }, + /* LAT: 30 */ { 3722, 3726, 3778, 3876, 4017, 4186, 4361, 4515, 4622, 4648, 4575, 4421, 4234, 4074, 3972, 3929, 3935, 3988, 4077, 4176, 4275, 4382, 4499, 4617, 4737, 4866, 4988, 5074, 5098, 5038, 4880, 4639, 4364, 4109, 3909, 3779, 3722, }, + /* LAT: 40 */ { 4222, 4216, 4278, 4399, 4564, 4750, 4932, 5088, 5192, 5218, 5151, 5004, 4820, 4649, 4524, 4451, 4427, 4452, 4515, 4595, 4683, 4781, 4897, 5030, 5182, 5345, 5497, 5606, 5643, 5586, 5428, 5190, 4916, 4657, 4446, 4299, 4222, }, + /* LAT: 50 */ { 4832, 4820, 4872, 4979, 5123, 5281, 5431, 5552, 5627, 5637, 5575, 5450, 5290, 5128, 4993, 4898, 4845, 4836, 4862, 4914, 4984, 5076, 5196, 5346, 5522, 5707, 5873, 5989, 6030, 5984, 5851, 5656, 5432, 5218, 5038, 4906, 4832, }, + /* LAT: 60 */ { 5393, 5376, 5400, 5459, 5543, 5637, 5724, 5792, 5827, 5820, 5768, 5675, 5556, 5430, 5312, 5218, 5153, 5120, 5119, 5148, 5205, 5293, 5412, 5559, 5726, 5894, 6040, 6142, 6183, 6160, 6077, 5951, 5807, 5665, 5543, 5450, 5393, }, + /* LAT: 70 */ { 5726, 5703, 5697, 5707, 5727, 5753, 5778, 5793, 5795, 5777, 5740, 5684, 5616, 5541, 5468, 5405, 5358, 5331, 5327, 5348, 5393, 5463, 5555, 5664, 5781, 5895, 5993, 6065, 6103, 6106, 6077, 6024, 5957, 5887, 5821, 5766, 5726, }, + /* LAT: 80 */ { 5790, 5772, 5756, 5743, 5733, 5723, 5713, 5702, 5688, 5670, 5649, 5625, 5599, 5573, 5549, 5529, 5516, 5511, 5517, 5532, 5558, 5594, 5636, 5684, 5734, 5782, 5824, 5858, 5882, 5895, 5897, 5889, 5875, 5855, 5833, 5811, 5790, }, /* LAT: 90 */ { 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, }, }; static constexpr float WMM_STRENGTH_MIN_GS = 22.2; // latitude: -30, longitude: -60 diff --git a/src/lib/world_magnetic_model/test_geo_lookup.cpp b/src/lib/world_magnetic_model/test_geo_lookup.cpp index 94c0b969ff14..f8dfe3e45bb3 100644 --- a/src/lib/world_magnetic_model/test_geo_lookup.cpp +++ b/src/lib/world_magnetic_model/test_geo_lookup.cpp @@ -43,7 +43,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-50, -180), 31.6, 0.40 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, -175), 31.8, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, -170), 31.8, 0.39 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-50, -165), 31.6, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, -165), 31.7, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, -160), 31.4, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, -155), 31.2, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, -150), 30.9, 0.37 + 1.0); @@ -61,7 +61,7 @@ TEST(GeoLookupTest, declination) 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, -75), 14.0, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, -70), 10.3, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, -65), 6.4, 0.40 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, -60), 2.6, 0.41 + 1.0); @@ -72,7 +72,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-50, -35), -10.7, 0.48 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, -30), -12.1, 0.49 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, -25), -13.3, 0.51 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-50, -20), -14.4, 0.52 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, -20), -14.3, 0.52 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, -15), -15.5, 0.53 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, -10), -16.9, 0.53 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, -5), -18.7, 0.53 + 1.0); @@ -84,7 +84,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-50, 25), -37.9, 0.46 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, 30), -41.5, 0.44 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, 35), -44.9, 0.43 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-50, 40), -48.0, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, 40), -48.1, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, 45), -50.8, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, 50), -53.2, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, 55), -55.1, 0.41 + 1.0); @@ -97,28 +97,28 @@ TEST(GeoLookupTest, declination) 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, 105), -37.3, 0.55 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, 110), -30.0, 0.57 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, 115), -21.8, 0.59 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, 120), -13.3, 0.59 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, 125), -5.1, 0.59 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-50, 130), 2.4, 0.57 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, 130), 2.5, 0.57 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, 135), 9.0, 0.55 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-50, 140), 14.5, 0.52 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, 140), 14.6, 0.52 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, 145), 19.1, 0.50 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, 150), 22.7, 0.48 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, 155), 25.6, 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, 160), 27.7, 0.45 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, 165), 29.3, 0.43 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, 170), 30.4, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, 175), 31.2, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, 180), 31.6, 0.40 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, -180), 26.6, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, -175), 26.9, 0.37 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-45, -170), 27.0, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, -170), 27.1, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, -165), 27.0, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, -160), 26.9, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-45, -155), 26.7, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, -155), 26.8, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, -150), 26.5, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, -145), 26.3, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, -140), 26.2, 0.36 + 1.0); @@ -129,22 +129,22 @@ TEST(GeoLookupTest, declination) 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, -100), 24.3, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, -95), 22.9, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, -90), 20.9, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, -85), 18.3, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, -80), 15.0, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, -75), 11.3, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, -70), 7.2, 0.40 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-45, -65), 3.0, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, -65), 2.9, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, -60), -1.2, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, -55), -5.0, 0.43 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, -50), -8.4, 0.44 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, -45), -11.1, 0.46 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, -40), -13.3, 0.48 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, -35), -14.9, 0.50 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-45, -30), -16.0, 0.51 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-45, -25), -16.9, 0.53 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, -30), -16.0, 0.52 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, -25), -16.8, 0.53 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, -20), -17.5, 0.55 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, -15), -18.1, 0.57 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, -10), -18.8, 0.58 + 1.0); @@ -157,7 +157,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-45, 25), -36.7, 0.49 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, 30), -40.1, 0.47 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, 35), -43.2, 0.45 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-45, 40), -45.9, 0.43 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, 40), -46.0, 0.43 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, 45), -48.2, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, 50), -50.0, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, 55), -51.3, 0.41 + 1.0); @@ -172,16 +172,16 @@ TEST(GeoLookupTest, declination) 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, 115), -12.2, 0.48 + 1.8); EXPECT_NEAR(get_mag_declination_degrees(-45, 120), -6.4, 0.47 + 1.8); EXPECT_NEAR(get_mag_declination_degrees(-45, 125), -1.1, 0.46 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, 130), 3.8, 0.45 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, 135), 8.2, 0.44 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, 140), 12.0, 0.43 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, 145), 15.4, 0.42 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-45, 150), 18.3, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, 150), 18.3, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, 155), 20.7, 0.41 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-45, 160), 22.6, 0.40 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, 160), 22.7, 0.40 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, 165), 24.2, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, 170), 25.3, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, 175), 26.1, 0.38 + 1.0); @@ -204,8 +204,8 @@ TEST(GeoLookupTest, declination) 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.1, 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, -90), 18.2, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, -85), 15.6, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, -80), 12.3, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, -75), 8.4, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, -70), 4.0, 0.40 + 1.0); @@ -216,13 +216,13 @@ TEST(GeoLookupTest, declination) 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, -30), -19.6, 0.52 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, -25), -20.3, 0.55 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, -20), -20.6, 0.57 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, -15), -20.8, 0.60 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, -10), -20.9, 0.62 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, -5), -21.2, 0.63 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-40, 0), -22.0, 0.63 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, 0), -21.9, 0.63 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, 5), -23.3, 0.62 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, 10), -25.4, 0.61 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, 15), -28.1, 0.58 + 1.0); @@ -232,9 +232,9 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-40, 35), -40.2, 0.46 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, 40), -42.5, 0.44 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, 45), -44.3, 0.43 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-40, 50), -45.5, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, 50), -45.6, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, 55), -46.1, 0.41 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-40, 60), -46.0, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, 60), -46.1, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, 65), -45.3, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, 70), -43.8, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, 75), -41.5, 0.41 + 1.0); @@ -256,16 +256,16 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-40, 155), 17.2, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, 160), 19.0, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, 165), 20.4, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-40, 170), 21.4, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, 170), 21.5, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, 175), 22.2, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, 180), 22.8, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, -180), 19.7, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, -175), 20.0, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, -170), 20.2, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, -165), 20.3, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-35, -160), 20.3, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, -160), 20.4, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, -155), 20.3, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-35, -150), 20.1, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, -150), 20.2, 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); @@ -280,27 +280,27 @@ TEST(GeoLookupTest, declination) 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.5, 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, -75), 5.4, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, -70), 0.9, 0.40 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, -65), -3.8, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, -60), -8.3, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, -55), -12.4, 0.43 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, -50), -15.7, 0.44 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-35, -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, -45), -18.4, 0.46 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, -40), -20.3, 0.48 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, -35), -21.6, 0.50 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, -30), -22.5, 0.52 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, -25), -23.0, 0.54 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, -20), -23.2, 0.57 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, -15), -23.0, 0.60 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, -10), -22.6, 0.62 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-35, -5), -22.2, 0.65 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, -5), -22.1, 0.65 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, 0), -21.9, 0.66 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, 5), -22.2, 0.66 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, 10), -23.3, 0.64 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, 15), -25.2, 0.61 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, 20), -27.6, 0.58 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-35, 25), -30.3, 0.54 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, 25), -30.4, 0.54 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, 30), -33.0, 0.50 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, 35), -35.4, 0.47 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, 40), -37.4, 0.45 + 1.0); @@ -310,7 +310,7 @@ TEST(GeoLookupTest, declination) 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, 75), -33.1, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, 80), -29.9, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, 85), -26.2, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, 90), -22.1, 0.39 + 1.0); @@ -327,7 +327,7 @@ TEST(GeoLookupTest, declination) 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, 160), 16.2, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, 165), 17.4, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, 170), 18.4, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, 175), 19.2, 0.34 + 1.0); @@ -365,28 +365,28 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-30, -30), -24.3, 0.50 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, -25), -24.6, 0.52 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, -20), -24.6, 0.55 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-30, -15), -24.2, 0.57 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, -15), -24.1, 0.57 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, -10), -23.3, 0.60 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-30, -5), -22.1, 0.63 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-30, 0), -20.8, 0.64 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-30, 5), -19.9, 0.65 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, -5), -22.0, 0.63 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, 0), -20.7, 0.64 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, 5), -19.8, 0.65 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, 10), -19.7, 0.63 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, 15), -20.5, 0.61 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, 20), -22.1, 0.58 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, 25), -24.2, 0.54 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, 30), -26.5, 0.50 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-30, 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, 35), -28.7, 0.47 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, 40), -30.5, 0.44 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, 45), -31.7, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, 50), -32.3, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, 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.0, 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, 75), -25.4, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, 80), -22.4, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, 85), -19.1, 0.37 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-30, 90), -15.6, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, 90), -15.5, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, 95), -12.0, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, 100), -8.7, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, 105), -5.8, 0.35 + 1.0); @@ -399,7 +399,7 @@ TEST(GeoLookupTest, declination) 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, 155), 12.6, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, 160), 13.9, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, 165), 15.1, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, 170), 16.0, 0.33 + 1.0); @@ -407,7 +407,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-30, 180), 17.2, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, -180), 15.2, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, -175), 15.4, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-25, -170), 15.5, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, -170), 15.6, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, -165), 15.6, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, -160), 15.7, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, -155), 15.7, 0.32 + 1.0); @@ -426,13 +426,13 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-25, -90), 10.3, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, -85), 7.7, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, -80), 4.2, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-25, -75), 0.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, -75), 0.0, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, -70), -4.6, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, -65), -9.2, 0.39 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-25, -60), -13.4, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, -60), -13.5, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, -55), -17.1, 0.40 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, -50), -20.0, 0.41 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-25, -45), -22.1, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, -45), -22.2, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, -40), -23.6, 0.44 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, -35), -24.5, 0.45 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, -30), -24.9, 0.47 + 1.0); @@ -441,15 +441,15 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-25, -15), -23.7, 0.53 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, -10), -22.3, 0.55 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, -5), -20.4, 0.57 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-25, 0), -18.3, 0.59 + 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, 0), -18.2, 0.59 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, 5), -16.3, 0.59 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, 10), -15.0, 0.58 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, 15), -14.7, 0.57 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, 20), -15.3, 0.54 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, 25), -16.8, 0.51 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, 30), -18.7, 0.48 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, 35), -20.7, 0.45 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-25, 40), -22.5, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, 40), -22.6, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, 45), -23.9, 0.40 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, 50), -24.7, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, 55), -24.8, 0.38 + 1.0); @@ -457,8 +457,8 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-25, 65), -22.9, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, 70), -21.2, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, 75), -19.0, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-25, 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, 80), -16.4, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, 85), -13.6, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, 90), -10.7, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, 95), -7.8, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, 100), -5.2, 0.33 + 1.0); @@ -476,7 +476,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-25, 160), 12.2, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, 165), 13.3, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, 170), 14.1, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-25, 175), 14.7, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, 175), 14.8, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, 180), 15.2, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, -180), 13.5, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, -175), 13.7, 0.31 + 1.0); @@ -495,34 +495,34 @@ TEST(GeoLookupTest, declination) 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.0, 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, -95), 9.9, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, -90), 8.1, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, -85), 5.5, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, -80), 2.0, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, -75), -2.1, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, -70), -6.6, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, -65), -11.0, 0.37 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-20, -60), -15.0, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, -60), -15.1, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, -55), -18.4, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, -50), -21.0, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, -45), -22.8, 0.40 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, -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, -25), -24.0, 0.45 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, -20), -23.2, 0.47 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, -15), -21.8, 0.48 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-20, -10), -19.9, 0.49 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-20, -5), -17.5, 0.50 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, -10), -19.8, 0.49 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, -5), -17.4, 0.50 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, 0), -14.9, 0.51 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, 5), -12.4, 0.51 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-20, 10), -10.5, 0.51 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, 10), -10.4, 0.51 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, 15), -9.3, 0.49 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, 20), -9.1, 0.48 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-20, 25), -9.8, 0.46 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, 25), -9.9, 0.46 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, 30), -11.3, 0.43 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, 35), -13.2, 0.41 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-20, 40), -15.0, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, 40), -15.1, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, 45), -16.6, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, 50), -17.6, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, 55), -18.0, 0.36 + 1.0); @@ -533,7 +533,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-20, 80), -11.9, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, 85), -9.7, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, 90), -7.3, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-20, 95), -5.0, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, 95), -4.9, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, 100), -2.9, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, 105), -1.3, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, 110), -0.2, 0.31 + 1.0); @@ -543,7 +543,7 @@ TEST(GeoLookupTest, declination) 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.5, 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, 145), 6.9, 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); @@ -567,17 +567,17 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-15, -115), 10.5, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, -110), 10.3, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, -105), 10.0, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-15, -100), 9.4, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, -100), 9.3, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, -95), 8.2, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, -90), 6.3, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, -85), 3.6, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-15, -80), 0.2, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, -80), 0.1, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, -75), -3.9, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, -70), -8.2, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, -65), -12.3, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-15, -60), -16.0, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, -60), -16.1, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, -55), -19.1, 0.37 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-15, -50), -21.3, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, -50), -21.4, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, -45), -22.8, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, -40), -23.5, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, -35), -23.6, 0.40 + 1.0); @@ -585,10 +585,10 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-15, -25), -22.1, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, -20), -20.7, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, -15), -18.8, 0.43 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-15, -10), -16.6, 0.44 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, -10), -16.5, 0.44 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, -5), -14.0, 0.44 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, 0), -11.4, 0.44 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-15, 5), -8.9, 0.44 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, 5), -8.8, 0.44 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, 10), -6.7, 0.43 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, 15), -5.2, 0.43 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, 20), -4.5, 0.42 + 1.0); @@ -600,12 +600,12 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-15, 50), -11.9, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, 55), -12.5, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, 60), -12.6, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-15, 65), -12.2, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, 65), -12.1, 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.6, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, 85), -6.9, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-15, 90), -5.0, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, 90), -4.9, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, 95), -3.1, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, 100), -1.5, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, 105), -0.3, 0.31 + 1.0); @@ -622,7 +622,7 @@ TEST(GeoLookupTest, declination) 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, 175), 12.0, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, 180), 12.2, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, -180), 11.3, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, -175), 11.3, 0.31 + 1.0); @@ -644,13 +644,13 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-10, -95), 6.8, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, -90), 4.8, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, -85), 2.1, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-10, -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, -80), -1.4, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, -75), -5.3, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, -70), -9.3, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-10, -65), -13.1, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, -65), -13.2, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, -60), -16.5, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-10, -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, -55), -19.3, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, -50), -21.2, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, -45), -22.2, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, -40), -22.5, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, -35), -22.1, 0.37 + 1.0); @@ -658,7 +658,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-10, -25), -19.6, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, -20), -17.7, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, -15), -15.5, 0.39 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-10, -10), -13.2, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, -10), -13.1, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, -5), -10.7, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, 0), -8.3, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, 5), -6.1, 0.38 + 1.0); @@ -687,17 +687,17 @@ TEST(GeoLookupTest, declination) 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, 135), 2.7, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, 140), 3.9, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, 145), 5.2, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, 150), 6.5, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, 155), 7.7, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, 160), 8.8, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, 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, 170), 10.6, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, 175), 11.0, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, 180), 11.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(-5, -180), 10.6, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, -175), 10.5, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, -170), 10.4, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, -165), 10.2, 0.31 + 1.0); @@ -709,33 +709,33 @@ TEST(GeoLookupTest, declination) 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, -120), 8.7, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, -115), 8.5, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, -110), 8.2, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, -105), 7.7, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, -100), 6.9, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, -95), 5.6, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-5, -90), 3.6, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, -90), 3.5, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, -85), 0.8, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-5, -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, -80), -2.6, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, -75), -6.3, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, -70), -10.1, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, -65), -13.6, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-5, -60), -16.6, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, -60), -16.7, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, -55), -19.0, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, -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, -40), -21.1, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, -35), -20.3, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, -30), -18.8, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, -25), -16.9, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-5, -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, -20), -14.7, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, -15), -12.4, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, -10), -10.1, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, -5), -7.9, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, 0), -5.9, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, 5), -4.0, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-5, 10), -2.3, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, 10), -2.2, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, 15), -0.8, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, 20), 0.3, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, 25), 0.7, 0.34 + 1.0); @@ -748,13 +748,13 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-5, 60), -5.7, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, 65), -5.8, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, 70), -5.7, 0.30 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-5, 75), -5.3, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, 75), -5.2, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, 80), -4.6, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, 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, 105), 0.3, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, 110), 0.6, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, 115), 0.5, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, 120), 0.4, 0.30 + 1.0); @@ -767,9 +767,9 @@ TEST(GeoLookupTest, declination) 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, 170), 9.9, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, 175), 10.3, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-5, 180), 10.5, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, 180), 10.6, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, -180), 10.0, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, -175), 10.0, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, -170), 9.8, 0.31 + 1.0); @@ -786,12 +786,12 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(0, -115), 7.9, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, -110), 7.5, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, -105), 7.0, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(0, -100), 6.1, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, -100), 6.0, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, -95), 4.6, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, -90), 2.5, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, -85), -0.3, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, -80), -3.5, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(0, -75), -7.0, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, -75), -7.1, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, -70), -10.6, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, -65), -13.8, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, -60), -16.5, 0.34 + 1.0); @@ -805,7 +805,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(0, -20), -12.2, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, -15), -9.9, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, -10), -7.7, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(0, -5), -5.8, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, -5), -5.7, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, 0), -4.0, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, 5), -2.4, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, 10), -1.0, 0.33 + 1.0); @@ -823,15 +823,15 @@ TEST(GeoLookupTest, declination) 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, 85), -2.7, 0.29 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, 90), -1.8, 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, 125), -0.2, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, 130), 0.3, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, 135), 1.1, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, 140), 2.3, 0.30 + 1.0); @@ -854,38 +854,38 @@ TEST(GeoLookupTest, declination) 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, -125), 8.3, 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.1, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -105), 6.4, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -100), 5.4, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(5, -95), 3.8, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, -95), 3.7, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -90), 1.5, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -85), -1.2, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -80), -4.4, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -75), -7.7, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(5, -70), -10.9, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, -70), -11.0, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -65), -13.9, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -60), -16.3, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(5, -55), -17.9, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, -55), -18.0, 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, -35), -16.5, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -30), -14.6, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -25), -12.4, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -20), -10.1, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(5, -15), -7.9, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, -15), -7.8, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -10), -5.8, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -5), -4.1, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, 0), -2.6, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, 5), -1.3, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(5, 10), -0.1, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, 10), -0.0, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, 15), 1.1, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, 20), 2.0, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, 25), 2.6, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(5, 30), 2.7, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, 30), 2.8, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, 35), 2.3, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, 40), 1.5, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, 45), 0.4, 0.30 + 1.0); @@ -933,10 +933,10 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(10, -110), 7.0, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, -105), 6.1, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, -100), 4.8, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(10, -95), 3.1, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, -95), 3.0, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, -90), 0.7, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, -85), -2.1, 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, -80), -5.2, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, -75), -8.3, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, -70), -11.3, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, -65), -13.9, 0.33 + 1.0); @@ -948,13 +948,13 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(10, -35), -14.9, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, -30), -12.9, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, -25), -10.7, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(10, -20), -8.5, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, -20), -8.4, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, -15), -6.3, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, -10), -4.4, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, -5), -2.8, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, 0), -1.5, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, 5), -0.4, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(10, 10), 0.6, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, 10), 0.7, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, 15), 1.6, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, 20), 2.5, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, 25), 3.1, 0.31 + 1.0); @@ -978,8 +978,8 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(10, 115), -1.4, 0.29 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, 120), -1.8, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, 125), -1.9, 0.30 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(10, 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, 130), -1.7, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, 135), -1.0, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, 140), 0.1, 0.30 + 1.0); 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); @@ -1007,7 +1007,7 @@ TEST(GeoLookupTest, declination) 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.0, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, -90), -0.0, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, -85), -2.8, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, -80), -5.9, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, -75), -8.9, 0.33 + 1.0); @@ -1017,17 +1017,17 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(15, -55), -16.5, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, -50), -16.7, 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, -40), -15.0, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, -35), -13.4, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, -30), -11.5, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(15, -25), -9.4, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, -25), -9.3, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, -20), -7.2, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(15, -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), -5.1, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, -10), -3.3, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, -5), -1.8, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, 0), -0.6, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, 5), 0.4, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, 10), 1.3, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, 15), 2.1, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, 20), 2.9, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, 25), 3.5, 0.30 + 1.0); @@ -1075,16 +1075,16 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(20, -130), 10.1, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, -125), 9.8, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, -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, -115), 8.4, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, -110), 7.4, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, -105), 6.1, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, -100), 4.3, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, -95), 2.1, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, -90), -0.6, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, -85), -3.5, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(20, -80), -6.5, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(20, -80), -6.6, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, -75), -9.4, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(20, -70), -11.9, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(20, -70), -12.0, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, -65), -13.9, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, -60), -15.3, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, -55), -15.9, 0.33 + 1.0); @@ -1092,14 +1092,14 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(20, -45), -15.1, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, -40), -13.8, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, -35), -12.2, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(20, -30), -10.4, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(20, -30), -10.3, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, -25), -8.3, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, -20), -6.3, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, -15), -4.3, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, -10), -2.6, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, -5), -1.1, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, 0), 0.0, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(20, 5), 0.9, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(20, 5), 1.0, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, 10), 1.8, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, 15), 2.5, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, 20), 3.2, 0.31 + 1.0); @@ -1110,7 +1110,7 @@ TEST(GeoLookupTest, declination) 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, 60), 1.0, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, 65), 0.6, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, 70), 0.3, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, 75), -0.0, 0.30 + 1.0); @@ -1120,9 +1120,9 @@ TEST(GeoLookupTest, declination) 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, 110), -2.4, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, 115), -3.1, 0.30 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(20, 120), -3.7, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(20, 120), -3.8, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, 125), -4.1, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, 130), -4.1, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, 135), -3.6, 0.30 + 1.0); @@ -1149,8 +1149,8 @@ TEST(GeoLookupTest, declination) 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, -110), 7.9, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, -105), 6.3, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, -100), 4.3, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, -95), 1.8, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, -90), -1.1, 0.34 + 1.0); @@ -1171,14 +1171,14 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(25, -15), -3.7, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, -10), -2.0, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, -5), -0.6, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(25, 0), 0.5, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, 0), 0.6, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, 5), 1.5, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, 10), 2.2, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(25, 15), 2.9, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, 15), 3.0, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, 20), 3.6, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, 25), 4.1, 0.31 + 1.0); 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, 35), 4.4, 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.9, 0.31 + 1.0); @@ -1222,11 +1222,11 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(30, -125), 11.7, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, -120), 11.0, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, -115), 10.0, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(30, -110), 8.6, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, -110), 8.6, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, -105), 6.7, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, -100), 4.4, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(30, -95), 1.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, -95), 1.6, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, -90), -1.5, 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.6, 0.35 + 1.0); @@ -1238,13 +1238,13 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(30, -45), -13.6, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, -40), -12.3, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, -35), -10.7, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(30, -30), -9.0, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, -30), -8.9, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, -25), -7.1, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, -20), -5.2, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(30, -15), -3.4, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, -15), -3.3, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, -10), -1.7, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, -5), -0.2, 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, 0), 1.0, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, 5), 1.9, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, 10), 2.7, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, 15), 3.4, 0.32 + 1.0); @@ -1293,7 +1293,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(35, -135), 13.3, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, -130), 13.2, 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, -120), 12.0, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, -115), 10.9, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, -110), 9.3, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, -105), 7.2, 0.36 + 1.0); @@ -1301,17 +1301,17 @@ TEST(GeoLookupTest, declination) 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, -80), -8.5, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, -75), -11.2, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, -70), -13.3, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, -65), -14.7, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, -60), -15.3, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, -55), -15.2, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(35, -50), -14.6, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, -50), -14.5, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, -45), -13.5, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, -40), -12.1, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, -35), -10.5, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(35, -30), -8.8, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, -30), -8.7, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, -25), -6.9, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, -20), -5.0, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, -15), -3.2, 0.33 + 1.0); @@ -1319,7 +1319,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(35, -5), -0.0, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, 0), 1.2, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, 5), 2.2, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(35, 10), 3.0, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, 10), 3.1, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, 15), 3.8, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, 20), 4.4, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, 25), 4.9, 0.33 + 1.0); @@ -1342,9 +1342,9 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(35, 110), -4.7, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, 115), -6.1, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, 120), -7.2, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(35, 125), -8.0, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, 125), -8.1, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, 130), -8.4, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(35, 135), -8.2, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, 135), -8.3, 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.6, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, 150), -5.2, 0.32 + 1.0); @@ -1356,11 +1356,11 @@ TEST(GeoLookupTest, declination) 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), 5.9, 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, -170), 7.5, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, -165), 9.0, 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, -150), 12.6, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, -145), 13.5, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, -140), 14.1, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, -135), 14.4, 0.36 + 1.0); @@ -1381,7 +1381,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(40, -60), -15.8, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, -55), -15.6, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, -50), -14.9, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(40, -45), -13.8, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, -45), -13.7, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, -40), -12.3, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, -35), -10.7, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, -30), -8.9, 0.34 + 1.0); @@ -1401,7 +1401,7 @@ TEST(GeoLookupTest, declination) 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, 55), 6.3, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, 60), 6.0, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, 65), 5.6, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, 70), 5.1, 0.34 + 1.0); @@ -1414,7 +1414,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(40, 105), -3.8, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, 110), -5.6, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, 115), -7.2, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(40, 120), -8.5, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, 120), -8.6, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, 125), -9.5, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, 130), -9.9, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, 135), -9.8, 0.33 + 1.0); @@ -1424,10 +1424,10 @@ TEST(GeoLookupTest, declination) 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, 170), 0.4, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, 175), 2.4, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, 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, -180), 3.2, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -175), 5.2, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -170), 7.1, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -165), 8.9, 0.35 + 1.0); @@ -1437,7 +1437,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(45, -145), 14.3, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -140), 15.0, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -135), 15.4, 0.37 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(45, -130), 15.5, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, -130), 15.4, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -125), 15.0, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -120), 14.2, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -115), 12.8, 0.39 + 1.0); @@ -1453,23 +1453,23 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(45, -65), -16.3, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -60), -16.8, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -55), -16.5, 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, -50), -15.7, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, -45), -14.5, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, -40), -13.0, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, -35), -11.3, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -30), -9.5, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -25), -7.6, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(45, -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, -20), -5.6, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, -15), -3.7, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -10), -1.9, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(45, -5), -0.3, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, -5), -0.2, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 0), 1.2, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 5), 2.5, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 10), 3.6, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 15), 4.6, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 20), 5.5, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 25), 6.3, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(45, 30), 6.9, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, 30), 7.0, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 35), 7.5, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 40), 7.9, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 45), 8.2, 0.36 + 1.0); @@ -1480,7 +1480,7 @@ TEST(GeoLookupTest, declination) 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, 85), 3.6, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 90), 1.9, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 95), -0.1, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 100), -2.2, 0.35 + 1.0); @@ -1488,7 +1488,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(45, 110), -6.5, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 115), -8.4, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 120), -9.9, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(45, 125), -10.9, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, 125), -11.0, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 130), -11.4, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 135), -11.3, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 140), -10.7, 0.34 + 1.0); @@ -1499,8 +1499,8 @@ TEST(GeoLookupTest, declination) 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.2, 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(45, 180), 3.2, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -180), 2.4, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, -175), 4.6, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, -170), 6.7, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, -165), 8.7, 0.37 + 1.0); @@ -1518,16 +1518,16 @@ TEST(GeoLookupTest, declination) 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, -90), -3.4, 0.45 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, -85), -7.7, 0.45 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, -80), -11.4, 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, -75), -14.4, 0.43 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, -70), -16.6, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, -65), -17.8, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, -60), -18.2, 0.40 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, -55), -17.9, 0.40 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, -50), -17.1, 0.39 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(50, -45), -15.9, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -45), -15.8, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, -40), -14.3, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, -35), -12.5, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, -30), -10.6, 0.38 + 1.0); @@ -1538,7 +1538,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(50, -5), -0.7, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, 0), 1.0, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, 5), 2.5, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(50, 10), 3.8, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, 10), 3.9, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, 15), 5.1, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, 20), 6.2, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, 25), 7.3, 0.39 + 1.0); @@ -1572,10 +1572,10 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(50, 165), -4.0, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, 170), -1.9, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, 175), 0.3, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(50, 180), 2.5, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, 180), 2.4, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, -180), 1.8, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, -175), 4.1, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(55, -170), 6.4, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -170), 6.3, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, -165), 8.6, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, -160), 10.7, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, -155), 12.6, 0.39 + 1.0); @@ -1584,7 +1584,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(55, -140), 16.8, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, -135), 17.5, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, -130), 17.8, 0.43 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(55, -125), 17.5, 0.45 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -125), 17.4, 0.45 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, -120), 16.5, 0.46 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, -115), 14.9, 0.48 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, -110), 12.4, 0.49 + 1.0); @@ -1598,23 +1598,23 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(55, -70), -18.7, 0.48 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, -65), -19.9, 0.46 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, -60), -20.3, 0.45 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(55, -55), -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, -55), -19.9, 0.44 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -50), -19.0, 0.43 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, -45), -17.7, 0.42 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(55, -40), -16.1, 0.42 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(55, -35), -14.2, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -40), -16.0, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -35), -14.1, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, -30), -12.1, 0.41 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(55, -25), -10.0, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -25), -9.9, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, -20), -7.8, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, -15), -5.6, 0.41 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(55, -10), -3.5, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -10), -3.4, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, -5), -1.4, 0.41 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(55, 0), 0.5, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, 0), 0.6, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, 5), 2.4, 0.41 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(55, 10), 4.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, 10), 4.1, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, 15), 5.7, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, 20), 7.2, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, 25), 8.6, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, 30), 9.9, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, 35), 11.1, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, 40), 12.1, 0.42 + 1.0); @@ -1625,21 +1625,21 @@ TEST(GeoLookupTest, declination) 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, 80), 9.6, 0.44 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, 85), 7.2, 0.44 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, 90), 4.4, 0.44 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, 95), 1.2, 0.44 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, 100), -2.2, 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, 105), -5.5, 0.44 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, 110), -8.4, 0.43 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, 115), -10.9, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, 120), -12.7, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, 125), -13.9, 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, 130), -14.4, 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.4, 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, 150), -10.9, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, 155), -9.1, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, 160), -7.1, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, 165), -5.0, 0.37 + 1.0); @@ -1648,9 +1648,9 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(55, 180), 1.8, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, -180), 1.2, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, -175), 3.6, 0.41 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, -170), 6.1, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -170), 6.0, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, -165), 8.4, 0.42 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, -160), 10.7, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -160), 10.6, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, -155), 12.8, 0.43 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, -150), 14.7, 0.44 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, -145), 16.3, 0.45 + 1.0); @@ -1660,50 +1660,50 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(60, -125), 18.7, 0.52 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, -120), 17.7, 0.54 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, -115), 15.9, 0.57 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, -110), 13.2, 0.60 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -110), 13.2, 0.59 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, -105), 9.4, 0.62 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, -100), 4.7, 0.64 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, -95), -0.8, 0.65 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, -90), -6.4, 0.65 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, -85), -11.6, 0.64 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, -80), -16.1, 0.62 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -80), -16.0, 0.62 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, -75), -19.4, 0.59 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, -70), -21.6, 0.56 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, -65), -22.8, 0.54 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, -60), -23.1, 0.52 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -60), -23.0, 0.52 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, -55), -22.6, 0.50 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, -50), -21.6, 0.48 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, -45), -20.1, 0.47 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, -40), -18.3, 0.47 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, -35), -16.3, 0.46 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, -30), -14.1, 0.45 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, -25), -11.8, 0.45 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, -20), -9.4, 0.45 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, -15), -7.0, 0.45 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, -10), -4.6, 0.45 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -25), -11.7, 0.45 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -20), -9.3, 0.45 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -15), -6.9, 0.45 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -10), -4.5, 0.45 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, -5), -2.2, 0.45 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, 0), 0.0, 0.45 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, 5), 2.2, 0.45 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, 0), 0.1, 0.45 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, 5), 2.3, 0.45 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, 10), 4.3, 0.45 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, 15), 6.3, 0.46 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, 15), 6.4, 0.46 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, 20), 8.3, 0.46 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, 25), 10.1, 0.46 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, 30), 11.9, 0.46 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, 35), 13.5, 0.46 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, 40), 14.9, 0.47 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, 40), 15.0, 0.47 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, 45), 16.2, 0.47 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, 50), 17.2, 0.47 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, 55), 17.8, 0.48 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, 60), 18.0, 0.49 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, 65), 17.7, 0.49 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, 65), 17.8, 0.49 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, 70), 16.9, 0.50 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, 75), 15.3, 0.51 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, 80), 13.1, 0.52 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, 85), 10.1, 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, 90), 6.5, 0.54 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, 95), 2.5, 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, 100), -1.7, 0.53 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, 105), -5.7, 0.52 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, 110), -9.2, 0.51 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, 115), -12.0, 0.50 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, 120), -14.1, 0.48 + 1.0); @@ -1717,7 +1717,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(60, 160), -8.1, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, 165), -5.9, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, 170), -3.6, 0.41 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, 175), -1.2, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, 175), -1.3, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, 180), 1.2, 0.41 + 1.0); } @@ -1743,14 +1743,14 @@ TEST(GeoLookupTest, inclination) 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, -80), -48.9, 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.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-50, -60), -49.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-50, -55), -50.6, 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, -50), -52.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, -45), -53.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-50, -40), -55.3, 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.2, 0.21 + 1.2); @@ -1803,11 +1803,11 @@ TEST(GeoLookupTest, inclination) 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, -145), -61.4, 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, -125), -57.5, 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); @@ -1819,11 +1819,11 @@ TEST(GeoLookupTest, inclination) 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), -45.0, 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, -65), -45.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, -60), -46.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, -55), -48.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, -50), -50.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -45), -52.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, -45), -52.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, -40), -54.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, -35), -56.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, -30), -58.7, 0.21 + 1.2); @@ -1838,11 +1838,11 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-45, 15), -61.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, 20), -60.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, 25), -60.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, 30), -59.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, 30), -59.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, 35), -60.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, 40), -60.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, 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, 50), -62.9, 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); @@ -1877,7 +1877,7 @@ TEST(GeoLookupTest, inclination) 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, -140), -56.4, 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); @@ -1889,15 +1889,15 @@ TEST(GeoLookupTest, inclination) 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.5, 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, -80), -41.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-40, -75), -41.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-40, -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, -55), -46.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-40, -50), -48.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, -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, -45), -51.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-40, -40), -54.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-40, -35), -56.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-40, -30), -58.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-40, -25), -60.8, 0.21 + 1.2); @@ -1963,17 +1963,17 @@ TEST(GeoLookupTest, inclination) 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, -75), -36.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-35, -70), -36.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-35, -65), -38.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -60), -40.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -60), -40.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-35, -55), -43.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-35, -50), -46.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -45), -49.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -45), -49.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-35, -40), -52.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -35), -55.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -35), -55.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-35, -30), -58.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -25), -60.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -25), -60.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-35, -20), -62.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-35, -15), -64.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-35, -10), -65.8, 0.21 + 1.2); @@ -1982,17 +1982,17 @@ TEST(GeoLookupTest, inclination) 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, 20), -64.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-35, 25), -63.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-35, 30), -62.0, 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, 35), -61.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-35, 40), -61.1, 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, 60), -64.3, 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, 70), -66.7, 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); @@ -2038,18 +2038,18 @@ TEST(GeoLookupTest, inclination) 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, -65), -33.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-30, -60), -36.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-30, -55), -39.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -50), -42.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, -50), -43.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-30, -45), -46.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-30, -40), -50.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -35), -53.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, -35), -53.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-30, -30), -56.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-30, -25), -59.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-30, -20), -61.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -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, -15), -63.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, -10), -65.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-30, -5), -66.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-30, 0), -67.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-30, 5), -67.8, 0.21 + 1.2); @@ -2064,7 +2064,7 @@ TEST(GeoLookupTest, inclination) 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, 65), -63.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-30, 70), -63.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-30, 75), -64.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-30, 80), -65.3, 0.21 + 1.2); @@ -2080,7 +2080,7 @@ TEST(GeoLookupTest, inclination) 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, 145), -61.5, 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.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-30, 160), -59.2, 0.21 + 1.2); @@ -2109,18 +2109,18 @@ TEST(GeoLookupTest, inclination) 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.8, 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, -75), -23.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, -70), -24.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, -65), -27.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -60), -30.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -60), -30.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, -55), -34.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, -50), -38.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, -45), -42.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, -40), -46.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -35), -50.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -35), -50.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, -30), -53.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, -25), -56.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -20), -59.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -20), -59.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, -15), -61.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, -10), -63.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, -5), -65.3, 0.21 + 1.2); @@ -2129,10 +2129,10 @@ TEST(GeoLookupTest, inclination) 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, 25), -62.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, 30), -60.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, 35), -59.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, 40), -58.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, 40), -58.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, 45), -57.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, 50), -57.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, 55), -57.8, 0.21 + 1.2); @@ -2145,7 +2145,7 @@ TEST(GeoLookupTest, inclination) 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, 105), -59.5, 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); @@ -2164,7 +2164,7 @@ TEST(GeoLookupTest, inclination) 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, -165), -38.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, -160), -37.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, -155), -36.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, -150), -34.8, 0.21 + 1.2); @@ -2187,14 +2187,14 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-20, -65), -20.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, -60), -23.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, -55), -28.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -50), -33.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, -50), -33.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, -45), -38.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, -40), -42.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, -35), -46.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -30), -50.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, -30), -50.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, -25), -53.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -20), -56.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, -20), -56.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, -15), -58.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, -10), -60.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, -5), -62.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, 0), -63.0, 0.21 + 1.2); @@ -2239,7 +2239,7 @@ TEST(GeoLookupTest, inclination) 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, -155), -27.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, -150), -26.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, -145), -25.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, -140), -23.8, 0.21 + 1.2); @@ -2247,29 +2247,29 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-15, -130), -21.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, -125), -20.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, -120), -18.9, 0.21 + 1.2); - 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, -115), -17.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -110), -16.4, 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, -75), -7.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -70), -9.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, -65), -12.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, -60), -16.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -55), -21.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -55), -21.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, -50), -26.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -45), -32.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -40), -37.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -45), -32.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -40), -37.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, -35), -41.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -30), -45.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -30), -45.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, -25), -48.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, -20), -51.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -15), -53.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -15), -54.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, -10), -55.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -5), -56.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -5), -57.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, 0), -57.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, 5), -57.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, 10), -57.5, 0.21 + 1.2); @@ -2284,7 +2284,7 @@ TEST(GeoLookupTest, inclination) 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, 70), -46.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, 75), -47.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, 80), -47.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, 85), -47.7, 0.21 + 1.2); @@ -2316,10 +2316,10 @@ TEST(GeoLookupTest, inclination) 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, -135), -13.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, -130), -11.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, -125), -10.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -120), -9.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, -120), -9.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, -115), -8.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, -110), -6.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, -105), -5.3, 0.21 + 1.2); @@ -2330,11 +2330,11 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-10, -80), 2.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, -75), 1.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, -70), -0.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -65), -3.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -60), -7.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -55), -13.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -50), -19.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -45), -24.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, -65), -3.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, -60), -8.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, -55), -13.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, -50), -19.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, -45), -24.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, -40), -30.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, -35), -35.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, -30), -39.4, 0.21 + 1.2); @@ -2347,15 +2347,15 @@ TEST(GeoLookupTest, inclination) 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, 20), -47.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, 25), -45.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, 30), -43.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, 35), -41.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, 40), -39.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, 40), -39.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, 45), -38.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, 50), -37.7, 0.21 + 1.2); 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, 60), -37.5, 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); @@ -2363,19 +2363,19 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-10, 85), -38.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, 90), -38.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, 95), -38.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, 100), -37.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, 100), -37.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, 105), -36.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, 110), -36.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, 115), -35.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, 120), -35.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, 125), -35.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, 125), -35.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, 130), -35.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, 135), -35.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, 140), -35.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, 145), -34.8, 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, 150), -34.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, 155), -33.1, 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, 160), -31.9, 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.1, 0.21 + 1.2); @@ -2389,37 +2389,37 @@ TEST(GeoLookupTest, inclination) 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.3, 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, -135), -3.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -130), -1.8, 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, -125), -0.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -120), 0.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -115), 1.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -110), 3.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -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, -90), 10.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -85), 11.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -80), 11.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -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.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -75), 10.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -70), 8.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -65), 5.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -60), 0.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -55), -4.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -50), -10.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -50), -10.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -45), -16.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -40), -22.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -35), -27.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -30), -31.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -25), -34.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -25), -35.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -20), -37.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -15), -39.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -15), -39.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -10), -40.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -5), -41.0, 0.21 + 1.2); 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, 15), -39.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, 20), -37.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, 25), -35.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, 30), -33.7, 0.21 + 1.2); @@ -2433,14 +2433,14 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-5, 70), -27.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, 75), -28.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, 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, 85), -28.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, 90), -28.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, 95), -28.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, 100), -27.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, 105), -27.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, 110), -26.2, 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, 115), -25.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, 120), -25.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, 125), -25.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, 130), -26.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, 135), -26.4, 0.21 + 1.2); @@ -2451,14 +2451,14 @@ TEST(GeoLookupTest, inclination) 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, 175), -17.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, 180), -15.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, -180), -5.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, -175), -3.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, -170), -1.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, -165), -0.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, -160), 1.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -155), 2.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, -155), 2.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, -150), 3.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, -145), 4.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, -140), 5.9, 0.21 + 1.2); @@ -2477,18 +2477,18 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(0, -75), 19.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, -70), 17.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, -65), 14.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -60), 9.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, -60), 9.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, -55), 4.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -50), -1.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, -50), -1.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, -45), -7.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, -40), -12.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -35), -17.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, -35), -18.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, -30), -22.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -25), -25.4, 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, -25), -25.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, -20), -27.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, -15), -29.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, -10), -30.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, -5), -30.4, 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); @@ -2498,16 +2498,16 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(0, 30), -22.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 35), -20.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 40), -18.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 45), -17.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, 45), -17.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 50), -16.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 55), -16.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 60), -16.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 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, 70), -16.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 75), -16.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 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, 80), -17.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, 85), -17.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, 90), -17.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 95), -17.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 100), -16.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 105), -16.0, 0.21 + 1.2); @@ -2517,13 +2517,13 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(0, 125), -15.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 130), -16.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 135), -16.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 140), -16.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, 140), -16.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 145), -16.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 150), -15.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 155), -14.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 160), -13.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 165), -11.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 170), -9.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, 170), -9.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 175), -7.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 180), -5.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, -180), 5.0, 0.21 + 1.2); @@ -2550,15 +2550,15 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(5, -75), 27.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, -70), 25.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, -65), 22.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -60), 18.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, -60), 18.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, -55), 13.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, -50), 8.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, -45), 2.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -40), -2.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -35), -7.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, -40), -2.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, -35), -7.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, -30), -11.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, -25), -14.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -20), -16.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, -20), -16.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, -15), -17.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, -10), -18.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, -5), -18.3, 0.21 + 1.2); @@ -2586,7 +2586,7 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(5, 105), -4.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, 110), -3.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, 115), -3.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 120), -4.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, 120), -3.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, 125), -4.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, 130), -5.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, 135), -5.9, 0.21 + 1.2); @@ -2618,19 +2618,19 @@ TEST(GeoLookupTest, inclination) 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, -85), 36.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, -80), 35.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, -75), 34.7, 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, -70), 32.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, -65), 30.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, -60), 26.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -55), 22.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, -55), 22.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, -50), 17.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -45), 12.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -40), 7.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, -45), 12.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, -40), 7.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, -35), 3.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -30), 0.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -25), -2.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, -30), -0.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, -25), -2.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, -20), -4.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, -15), -5.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, -10), -5.7, 0.21 + 1.2); @@ -2684,7 +2684,7 @@ TEST(GeoLookupTest, inclination) 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, -120), 36.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, -115), 37.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, -110), 38.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, -105), 39.9, 0.21 + 1.2); @@ -2693,17 +2693,17 @@ TEST(GeoLookupTest, inclination) 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.2, 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, -75), 41.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, -70), 39.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, -65), 36.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, -60), 33.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, -55), 30.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, -50), 26.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -45), 22.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, -45), 21.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, -40), 18.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, -35), 14.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -30), 11.6, 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, -30), 11.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, -25), 9.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, -20), 7.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, -15), 7.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, -10), 7.0, 0.21 + 1.2); @@ -2713,7 +2713,7 @@ TEST(GeoLookupTest, inclination) 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, 25), 13.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, 30), 14.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, 35), 16.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, 40), 17.9, 0.21 + 1.2); @@ -2721,10 +2721,10 @@ TEST(GeoLookupTest, inclination) 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), 20.0, 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, 65), 20.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, 70), 19.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, 75), 19.7, 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, 80), 19.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, 85), 19.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, 90), 19.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, 95), 18.9, 0.21 + 1.2); @@ -2739,7 +2739,7 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(15, 140), 15.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, 145), 14.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, 150), 14.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, 155), 15.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, 155), 15.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, 160), 16.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, 165), 17.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, 170), 19.5, 0.21 + 1.2); @@ -2764,19 +2764,19 @@ TEST(GeoLookupTest, inclination) 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, -85), 48.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, -80), 48.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, -75), 46.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, -70), 45.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, -65), 43.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, -60), 40.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, -55), 37.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -50), 34.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -45), 30.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, -50), 34.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, -45), 30.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, -40), 27.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, -35), 24.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, -30), 22.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -25), 20.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, -25), 20.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, -20), 19.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, -15), 19.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, -10), 19.0, 0.21 + 1.2); @@ -2799,12 +2799,12 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(20, 75), 30.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, 80), 30.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, 85), 30.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 90), 29.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, 90), 29.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, 95), 29.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, 100), 29.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 105), 29.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, 105), 29.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, 110), 29.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 115), 29.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, 115), 29.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, 120), 28.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, 125), 27.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, 130), 26.6, 0.21 + 1.2); @@ -2835,23 +2835,23 @@ TEST(GeoLookupTest, inclination) 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, -95), 53.7, 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.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, -80), 53.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, -75), 52.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, -70), 50.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -65), 48.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, -65), 48.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, -60), 46.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -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, -55), 43.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, -50), 41.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, -45), 38.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, -40), 36.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, -35), 34.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, -30), 32.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, -25), 31.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, -20), 30.2, 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, -15), 29.8, 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); @@ -2873,17 +2873,17 @@ TEST(GeoLookupTest, inclination) 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, 95), 39.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, 100), 39.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, 105), 39.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, 110), 38.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, 115), 38.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, 120), 37.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 125), 36.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, 125), 36.8, 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, 145), 33.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, 150), 32.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, 155), 32.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, 160), 33.2, 0.21 + 1.2); @@ -2911,13 +2911,13 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(30, -95), 58.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, -90), 58.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, -85), 58.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -80), 58.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, -80), 57.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, -75), 56.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, -70), 55.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -65), 53.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, -65), 53.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, -60), 51.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, -55), 49.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -50), 47.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, -50), 47.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, -45), 45.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, -40), 44.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, -35), 42.4, 0.21 + 1.2); @@ -2941,7 +2941,7 @@ TEST(GeoLookupTest, inclination) 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, 70), 47.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, 75), 47.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, 80), 47.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, 85), 47.8, 0.21 + 1.2); @@ -2960,7 +2960,7 @@ TEST(GeoLookupTest, inclination) 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, 165), 40.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, 170), 41.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, 175), 42.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, 180), 43.4, 0.21 + 1.2); @@ -2974,7 +2974,7 @@ TEST(GeoLookupTest, inclination) 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, -130), 57.3, 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); @@ -2983,7 +2983,7 @@ TEST(GeoLookupTest, inclination) 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, -85), 62.9, 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.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(35, -70), 60.0, 0.21 + 1.2); @@ -2993,7 +2993,7 @@ TEST(GeoLookupTest, inclination) 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.7, 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, -35), 49.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(35, -30), 48.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(35, -25), 47.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(35, -20), 47.5, 0.21 + 1.2); @@ -3018,7 +3018,7 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(35, 75), 54.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(35, 80), 54.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(35, 85), 54.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, 90), 54.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, 90), 54.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(35, 95), 54.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(35, 100), 54.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(35, 105), 54.5, 0.21 + 1.2); @@ -3057,14 +3057,14 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(40, -95), 67.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(40, -90), 67.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(40, -85), 67.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, -80), 66.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, -80), 66.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(40, -75), 65.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(40, -70), 64.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, -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, -65), 62.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, -60), 61.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(40, -55), 60.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, -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, -50), 58.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, -45), 57.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(40, -40), 56.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(40, -35), 55.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(40, -30), 55.0, 0.21 + 1.2); @@ -3088,7 +3088,7 @@ TEST(GeoLookupTest, inclination) 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.5, 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, 75), 60.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(40, 80), 60.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(40, 85), 60.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(40, 90), 61.0, 0.21 + 1.2); @@ -3127,10 +3127,10 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(45, -110), 69.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(45, -105), 70.0, 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, -95), 70.9, 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.7, 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, -80), 70.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(45, -75), 69.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(45, -70), 68.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(45, -65), 67.1, 0.21 + 1.2); @@ -3166,7 +3166,7 @@ TEST(GeoLookupTest, inclination) 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.4, 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, 100), 66.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(45, 105), 66.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(45, 110), 65.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(45, 115), 64.9, 0.21 + 1.2); @@ -3198,7 +3198,7 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(50, -120), 71.1, 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), 72.9, 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, -105), 73.6, 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); @@ -3338,14 +3338,14 @@ TEST(GeoLookupTest, inclination) 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, -135), 75.4, 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.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(60, -115), 78.8, 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, -110), 79.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(60, -105), 80.1, 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, -100), 80.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(60, -95), 80.7, 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); @@ -3376,7 +3376,7 @@ TEST(GeoLookupTest, inclination) 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, 55), 75.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(60, 60), 76.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(60, 65), 76.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(60, 70), 77.1, 0.21 + 1.2); @@ -3399,1690 +3399,1690 @@ TEST(GeoLookupTest, inclination) 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, 170), 70.7, 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, 58387, 145 + 584); - EXPECT_NEAR(get_mag_strength_tesla(-50, -175) * 1e9, 57235, 145 + 572); - EXPECT_NEAR(get_mag_strength_tesla(-50, -170) * 1e9, 56071, 145 + 561); - EXPECT_NEAR(get_mag_strength_tesla(-50, -165) * 1e9, 54902, 145 + 549); - EXPECT_NEAR(get_mag_strength_tesla(-50, -160) * 1e9, 53734, 145 + 537); - EXPECT_NEAR(get_mag_strength_tesla(-50, -155) * 1e9, 52572, 145 + 526); - EXPECT_NEAR(get_mag_strength_tesla(-50, -150) * 1e9, 51414, 145 + 514); - EXPECT_NEAR(get_mag_strength_tesla(-50, -145) * 1e9, 50254, 145 + 503); - EXPECT_NEAR(get_mag_strength_tesla(-50, -140) * 1e9, 49080, 145 + 491); - EXPECT_NEAR(get_mag_strength_tesla(-50, -135) * 1e9, 47876, 145 + 479); - EXPECT_NEAR(get_mag_strength_tesla(-50, -130) * 1e9, 46624, 145 + 466); - EXPECT_NEAR(get_mag_strength_tesla(-50, -125) * 1e9, 45307, 145 + 453); - EXPECT_NEAR(get_mag_strength_tesla(-50, -120) * 1e9, 43913, 145 + 439); - EXPECT_NEAR(get_mag_strength_tesla(-50, -115) * 1e9, 42435, 145 + 424); - EXPECT_NEAR(get_mag_strength_tesla(-50, -110) * 1e9, 40878, 145 + 409); - EXPECT_NEAR(get_mag_strength_tesla(-50, -105) * 1e9, 39256, 145 + 393); - EXPECT_NEAR(get_mag_strength_tesla(-50, -100) * 1e9, 37592, 145 + 376); - EXPECT_NEAR(get_mag_strength_tesla(-50, -95) * 1e9, 35921, 145 + 359); - EXPECT_NEAR(get_mag_strength_tesla(-50, -90) * 1e9, 34281, 145 + 343); - EXPECT_NEAR(get_mag_strength_tesla(-50, -85) * 1e9, 32717, 145 + 327); - EXPECT_NEAR(get_mag_strength_tesla(-50, -80) * 1e9, 31272, 145 + 313); - EXPECT_NEAR(get_mag_strength_tesla(-50, -75) * 1e9, 29987, 145 + 300); - EXPECT_NEAR(get_mag_strength_tesla(-50, -70) * 1e9, 28888, 145 + 289); - EXPECT_NEAR(get_mag_strength_tesla(-50, -65) * 1e9, 27991, 145 + 280); - EXPECT_NEAR(get_mag_strength_tesla(-50, -60) * 1e9, 27291, 145 + 273); - EXPECT_NEAR(get_mag_strength_tesla(-50, -55) * 1e9, 26769, 145 + 268); - EXPECT_NEAR(get_mag_strength_tesla(-50, -50) * 1e9, 26391, 145 + 264); - EXPECT_NEAR(get_mag_strength_tesla(-50, -45) * 1e9, 26117, 145 + 261); - EXPECT_NEAR(get_mag_strength_tesla(-50, -40) * 1e9, 25908, 145 + 259); - EXPECT_NEAR(get_mag_strength_tesla(-50, -35) * 1e9, 25728, 145 + 257); - EXPECT_NEAR(get_mag_strength_tesla(-50, -30) * 1e9, 25555, 145 + 256); - EXPECT_NEAR(get_mag_strength_tesla(-50, -25) * 1e9, 25380, 145 + 254); - EXPECT_NEAR(get_mag_strength_tesla(-50, -20) * 1e9, 25206, 145 + 252); - EXPECT_NEAR(get_mag_strength_tesla(-50, -15) * 1e9, 25049, 145 + 250); - EXPECT_NEAR(get_mag_strength_tesla(-50, -10) * 1e9, 24934, 145 + 249); - EXPECT_NEAR(get_mag_strength_tesla(-50, -5) * 1e9, 24899, 145 + 249); - EXPECT_NEAR(get_mag_strength_tesla(-50, 0) * 1e9, 24988, 145 + 250); - EXPECT_NEAR(get_mag_strength_tesla(-50, 5) * 1e9, 25250, 145 + 253); - EXPECT_NEAR(get_mag_strength_tesla(-50, 10) * 1e9, 25732, 145 + 257); - EXPECT_NEAR(get_mag_strength_tesla(-50, 15) * 1e9, 26474, 145 + 265); - EXPECT_NEAR(get_mag_strength_tesla(-50, 20) * 1e9, 27503, 145 + 275); + EXPECT_NEAR(get_mag_strength_tesla(-50, -180) * 1e9, 58381, 145 + 584); + EXPECT_NEAR(get_mag_strength_tesla(-50, -175) * 1e9, 57229, 145 + 572); + EXPECT_NEAR(get_mag_strength_tesla(-50, -170) * 1e9, 56063, 145 + 561); + EXPECT_NEAR(get_mag_strength_tesla(-50, -165) * 1e9, 54893, 145 + 549); + EXPECT_NEAR(get_mag_strength_tesla(-50, -160) * 1e9, 53726, 145 + 537); + EXPECT_NEAR(get_mag_strength_tesla(-50, -155) * 1e9, 52562, 145 + 526); + EXPECT_NEAR(get_mag_strength_tesla(-50, -150) * 1e9, 51404, 145 + 514); + EXPECT_NEAR(get_mag_strength_tesla(-50, -145) * 1e9, 50243, 145 + 502); + EXPECT_NEAR(get_mag_strength_tesla(-50, -140) * 1e9, 49068, 145 + 491); + EXPECT_NEAR(get_mag_strength_tesla(-50, -135) * 1e9, 47864, 145 + 479); + EXPECT_NEAR(get_mag_strength_tesla(-50, -130) * 1e9, 46612, 145 + 466); + EXPECT_NEAR(get_mag_strength_tesla(-50, -125) * 1e9, 45295, 145 + 453); + EXPECT_NEAR(get_mag_strength_tesla(-50, -120) * 1e9, 43900, 145 + 439); + EXPECT_NEAR(get_mag_strength_tesla(-50, -115) * 1e9, 42422, 145 + 424); + EXPECT_NEAR(get_mag_strength_tesla(-50, -110) * 1e9, 40865, 145 + 409); + EXPECT_NEAR(get_mag_strength_tesla(-50, -105) * 1e9, 39243, 145 + 392); + EXPECT_NEAR(get_mag_strength_tesla(-50, -100) * 1e9, 37579, 145 + 376); + EXPECT_NEAR(get_mag_strength_tesla(-50, -95) * 1e9, 35908, 145 + 359); + EXPECT_NEAR(get_mag_strength_tesla(-50, -90) * 1e9, 34268, 145 + 343); + EXPECT_NEAR(get_mag_strength_tesla(-50, -85) * 1e9, 32705, 145 + 327); + EXPECT_NEAR(get_mag_strength_tesla(-50, -80) * 1e9, 31261, 145 + 313); + EXPECT_NEAR(get_mag_strength_tesla(-50, -75) * 1e9, 29976, 145 + 300); + EXPECT_NEAR(get_mag_strength_tesla(-50, -70) * 1e9, 28878, 145 + 289); + EXPECT_NEAR(get_mag_strength_tesla(-50, -65) * 1e9, 27982, 145 + 280); + EXPECT_NEAR(get_mag_strength_tesla(-50, -60) * 1e9, 27283, 145 + 273); + EXPECT_NEAR(get_mag_strength_tesla(-50, -55) * 1e9, 26762, 145 + 268); + EXPECT_NEAR(get_mag_strength_tesla(-50, -50) * 1e9, 26384, 145 + 264); + EXPECT_NEAR(get_mag_strength_tesla(-50, -45) * 1e9, 26111, 145 + 261); + EXPECT_NEAR(get_mag_strength_tesla(-50, -40) * 1e9, 25901, 145 + 259); + EXPECT_NEAR(get_mag_strength_tesla(-50, -35) * 1e9, 25721, 145 + 257); + EXPECT_NEAR(get_mag_strength_tesla(-50, -30) * 1e9, 25548, 145 + 255); + EXPECT_NEAR(get_mag_strength_tesla(-50, -25) * 1e9, 25373, 145 + 254); + EXPECT_NEAR(get_mag_strength_tesla(-50, -20) * 1e9, 25198, 145 + 252); + EXPECT_NEAR(get_mag_strength_tesla(-50, -15) * 1e9, 25040, 145 + 250); + EXPECT_NEAR(get_mag_strength_tesla(-50, -10) * 1e9, 24925, 145 + 249); + EXPECT_NEAR(get_mag_strength_tesla(-50, -5) * 1e9, 24890, 145 + 249); + EXPECT_NEAR(get_mag_strength_tesla(-50, 0) * 1e9, 24979, 145 + 250); + EXPECT_NEAR(get_mag_strength_tesla(-50, 5) * 1e9, 25242, 145 + 252); + EXPECT_NEAR(get_mag_strength_tesla(-50, 10) * 1e9, 25725, 145 + 257); + EXPECT_NEAR(get_mag_strength_tesla(-50, 15) * 1e9, 26469, 145 + 265); + EXPECT_NEAR(get_mag_strength_tesla(-50, 20) * 1e9, 27500, 145 + 275); EXPECT_NEAR(get_mag_strength_tesla(-50, 25) * 1e9, 28830, 145 + 288); - EXPECT_NEAR(get_mag_strength_tesla(-50, 30) * 1e9, 30448, 145 + 304); - EXPECT_NEAR(get_mag_strength_tesla(-50, 35) * 1e9, 32333, 145 + 323); - EXPECT_NEAR(get_mag_strength_tesla(-50, 40) * 1e9, 34448, 145 + 344); - EXPECT_NEAR(get_mag_strength_tesla(-50, 45) * 1e9, 36750, 145 + 368); - EXPECT_NEAR(get_mag_strength_tesla(-50, 50) * 1e9, 39188, 145 + 392); - EXPECT_NEAR(get_mag_strength_tesla(-50, 55) * 1e9, 41710, 145 + 417); - EXPECT_NEAR(get_mag_strength_tesla(-50, 60) * 1e9, 44270, 145 + 443); - EXPECT_NEAR(get_mag_strength_tesla(-50, 65) * 1e9, 46822, 145 + 468); - EXPECT_NEAR(get_mag_strength_tesla(-50, 70) * 1e9, 49329, 145 + 493); - EXPECT_NEAR(get_mag_strength_tesla(-50, 75) * 1e9, 51756, 145 + 518); - EXPECT_NEAR(get_mag_strength_tesla(-50, 80) * 1e9, 54071, 145 + 541); - EXPECT_NEAR(get_mag_strength_tesla(-50, 85) * 1e9, 56245, 145 + 562); - EXPECT_NEAR(get_mag_strength_tesla(-50, 90) * 1e9, 58248, 145 + 582); - EXPECT_NEAR(get_mag_strength_tesla(-50, 95) * 1e9, 60050, 145 + 601); - EXPECT_NEAR(get_mag_strength_tesla(-50, 100) * 1e9, 61627, 145 + 616); - EXPECT_NEAR(get_mag_strength_tesla(-50, 105) * 1e9, 62957, 145 + 630); - EXPECT_NEAR(get_mag_strength_tesla(-50, 110) * 1e9, 64030, 145 + 640); - EXPECT_NEAR(get_mag_strength_tesla(-50, 115) * 1e9, 64841, 145 + 648); - EXPECT_NEAR(get_mag_strength_tesla(-50, 120) * 1e9, 65396, 145 + 654); - EXPECT_NEAR(get_mag_strength_tesla(-50, 125) * 1e9, 65706, 145 + 657); - EXPECT_NEAR(get_mag_strength_tesla(-50, 130) * 1e9, 65789, 145 + 658); - EXPECT_NEAR(get_mag_strength_tesla(-50, 135) * 1e9, 65663, 145 + 657); - EXPECT_NEAR(get_mag_strength_tesla(-50, 140) * 1e9, 65348, 145 + 653); - EXPECT_NEAR(get_mag_strength_tesla(-50, 145) * 1e9, 64865, 145 + 649); - EXPECT_NEAR(get_mag_strength_tesla(-50, 150) * 1e9, 64232, 145 + 642); - EXPECT_NEAR(get_mag_strength_tesla(-50, 155) * 1e9, 63470, 145 + 635); - EXPECT_NEAR(get_mag_strength_tesla(-50, 160) * 1e9, 62598, 145 + 626); - EXPECT_NEAR(get_mag_strength_tesla(-50, 165) * 1e9, 61636, 145 + 616); - EXPECT_NEAR(get_mag_strength_tesla(-50, 170) * 1e9, 60602, 145 + 606); - EXPECT_NEAR(get_mag_strength_tesla(-50, 175) * 1e9, 59513, 145 + 595); - EXPECT_NEAR(get_mag_strength_tesla(-50, 180) * 1e9, 58387, 145 + 584); - EXPECT_NEAR(get_mag_strength_tesla(-45, -180) * 1e9, 56234, 145 + 956); - EXPECT_NEAR(get_mag_strength_tesla(-45, -175) * 1e9, 55028, 145 + 935); - EXPECT_NEAR(get_mag_strength_tesla(-45, -170) * 1e9, 53814, 145 + 915); - EXPECT_NEAR(get_mag_strength_tesla(-45, -165) * 1e9, 52599, 145 + 894); - EXPECT_NEAR(get_mag_strength_tesla(-45, -160) * 1e9, 51392, 145 + 874); - EXPECT_NEAR(get_mag_strength_tesla(-45, -155) * 1e9, 50196, 145 + 853); - EXPECT_NEAR(get_mag_strength_tesla(-45, -150) * 1e9, 49013, 145 + 833); - EXPECT_NEAR(get_mag_strength_tesla(-45, -145) * 1e9, 47838, 145 + 813); - EXPECT_NEAR(get_mag_strength_tesla(-45, -140) * 1e9, 46660, 145 + 793); - EXPECT_NEAR(get_mag_strength_tesla(-45, -135) * 1e9, 45464, 145 + 773); - EXPECT_NEAR(get_mag_strength_tesla(-45, -130) * 1e9, 44228, 145 + 752); - EXPECT_NEAR(get_mag_strength_tesla(-45, -125) * 1e9, 42934, 145 + 730); - EXPECT_NEAR(get_mag_strength_tesla(-45, -120) * 1e9, 41566, 145 + 707); - EXPECT_NEAR(get_mag_strength_tesla(-45, -115) * 1e9, 40112, 145 + 682); - EXPECT_NEAR(get_mag_strength_tesla(-45, -110) * 1e9, 38574, 145 + 656); - EXPECT_NEAR(get_mag_strength_tesla(-45, -105) * 1e9, 36962, 145 + 628); - EXPECT_NEAR(get_mag_strength_tesla(-45, -100) * 1e9, 35296, 145 + 600); - EXPECT_NEAR(get_mag_strength_tesla(-45, -95) * 1e9, 33610, 145 + 571); - EXPECT_NEAR(get_mag_strength_tesla(-45, -90) * 1e9, 31949, 145 + 543); - EXPECT_NEAR(get_mag_strength_tesla(-45, -85) * 1e9, 30362, 145 + 516); - EXPECT_NEAR(get_mag_strength_tesla(-45, -80) * 1e9, 28906, 145 + 491); - EXPECT_NEAR(get_mag_strength_tesla(-45, -75) * 1e9, 27631, 145 + 470); - EXPECT_NEAR(get_mag_strength_tesla(-45, -70) * 1e9, 26576, 145 + 452); - EXPECT_NEAR(get_mag_strength_tesla(-45, -65) * 1e9, 25762, 145 + 438); - EXPECT_NEAR(get_mag_strength_tesla(-45, -60) * 1e9, 25184, 145 + 428); - EXPECT_NEAR(get_mag_strength_tesla(-45, -55) * 1e9, 24814, 145 + 422); - EXPECT_NEAR(get_mag_strength_tesla(-45, -50) * 1e9, 24608, 145 + 418); - EXPECT_NEAR(get_mag_strength_tesla(-45, -45) * 1e9, 24510, 145 + 417); - EXPECT_NEAR(get_mag_strength_tesla(-45, -40) * 1e9, 24467, 145 + 416); - EXPECT_NEAR(get_mag_strength_tesla(-45, -35) * 1e9, 24435, 145 + 415); - EXPECT_NEAR(get_mag_strength_tesla(-45, -30) * 1e9, 24386, 145 + 415); - EXPECT_NEAR(get_mag_strength_tesla(-45, -25) * 1e9, 24305, 145 + 413); - EXPECT_NEAR(get_mag_strength_tesla(-45, -20) * 1e9, 24193, 145 + 411); - EXPECT_NEAR(get_mag_strength_tesla(-45, -15) * 1e9, 24062, 145 + 409); - EXPECT_NEAR(get_mag_strength_tesla(-45, -10) * 1e9, 23936, 145 + 407); - EXPECT_NEAR(get_mag_strength_tesla(-45, -5) * 1e9, 23851, 145 + 405); - EXPECT_NEAR(get_mag_strength_tesla(-45, 0) * 1e9, 23856, 145 + 406); - EXPECT_NEAR(get_mag_strength_tesla(-45, 5) * 1e9, 24012, 145 + 408); - EXPECT_NEAR(get_mag_strength_tesla(-45, 10) * 1e9, 24384, 145 + 415); - EXPECT_NEAR(get_mag_strength_tesla(-45, 15) * 1e9, 25030, 145 + 426); - EXPECT_NEAR(get_mag_strength_tesla(-45, 20) * 1e9, 25997, 145 + 442); - EXPECT_NEAR(get_mag_strength_tesla(-45, 25) * 1e9, 27308, 145 + 464); - EXPECT_NEAR(get_mag_strength_tesla(-45, 30) * 1e9, 28959, 145 + 492); - EXPECT_NEAR(get_mag_strength_tesla(-45, 35) * 1e9, 30923, 145 + 526); - EXPECT_NEAR(get_mag_strength_tesla(-45, 40) * 1e9, 33155, 145 + 564); - EXPECT_NEAR(get_mag_strength_tesla(-45, 45) * 1e9, 35595, 145 + 605); - EXPECT_NEAR(get_mag_strength_tesla(-45, 50) * 1e9, 38179, 145 + 649); - EXPECT_NEAR(get_mag_strength_tesla(-45, 55) * 1e9, 40840, 145 + 694); - EXPECT_NEAR(get_mag_strength_tesla(-45, 60) * 1e9, 43518, 145 + 740); - EXPECT_NEAR(get_mag_strength_tesla(-45, 65) * 1e9, 46160, 145 + 785); - EXPECT_NEAR(get_mag_strength_tesla(-45, 70) * 1e9, 48722, 145 + 828); - EXPECT_NEAR(get_mag_strength_tesla(-45, 75) * 1e9, 51173, 145 + 870); - EXPECT_NEAR(get_mag_strength_tesla(-45, 80) * 1e9, 53483, 145 + 909); - EXPECT_NEAR(get_mag_strength_tesla(-45, 85) * 1e9, 55626, 145 + 946); - EXPECT_NEAR(get_mag_strength_tesla(-45, 90) * 1e9, 57574, 145 + 979); - EXPECT_NEAR(get_mag_strength_tesla(-45, 95) * 1e9, 59302, 145 + 1008); - EXPECT_NEAR(get_mag_strength_tesla(-45, 100) * 1e9, 60787, 145 + 1033); - EXPECT_NEAR(get_mag_strength_tesla(-45, 105) * 1e9, 62013, 145 + 1054); - EXPECT_NEAR(get_mag_strength_tesla(-45, 110) * 1e9, 62973, 145 + 1071); - EXPECT_NEAR(get_mag_strength_tesla(-45, 115) * 1e9, 63670, 145 + 1082); - EXPECT_NEAR(get_mag_strength_tesla(-45, 120) * 1e9, 64114, 145 + 1090); - EXPECT_NEAR(get_mag_strength_tesla(-45, 125) * 1e9, 64322, 145 + 1093); - EXPECT_NEAR(get_mag_strength_tesla(-45, 130) * 1e9, 64311, 145 + 1093); - EXPECT_NEAR(get_mag_strength_tesla(-45, 135) * 1e9, 64101, 145 + 1090); + EXPECT_NEAR(get_mag_strength_tesla(-50, 30) * 1e9, 30449, 145 + 304); + EXPECT_NEAR(get_mag_strength_tesla(-50, 35) * 1e9, 32336, 145 + 323); + EXPECT_NEAR(get_mag_strength_tesla(-50, 40) * 1e9, 34454, 145 + 345); + EXPECT_NEAR(get_mag_strength_tesla(-50, 45) * 1e9, 36757, 145 + 368); + EXPECT_NEAR(get_mag_strength_tesla(-50, 50) * 1e9, 39196, 145 + 392); + EXPECT_NEAR(get_mag_strength_tesla(-50, 55) * 1e9, 41719, 145 + 417); + EXPECT_NEAR(get_mag_strength_tesla(-50, 60) * 1e9, 44280, 145 + 443); + EXPECT_NEAR(get_mag_strength_tesla(-50, 65) * 1e9, 46832, 145 + 468); + EXPECT_NEAR(get_mag_strength_tesla(-50, 70) * 1e9, 49338, 145 + 493); + EXPECT_NEAR(get_mag_strength_tesla(-50, 75) * 1e9, 51765, 145 + 518); + EXPECT_NEAR(get_mag_strength_tesla(-50, 80) * 1e9, 54080, 145 + 541); + EXPECT_NEAR(get_mag_strength_tesla(-50, 85) * 1e9, 56254, 145 + 563); + EXPECT_NEAR(get_mag_strength_tesla(-50, 90) * 1e9, 58256, 145 + 583); + EXPECT_NEAR(get_mag_strength_tesla(-50, 95) * 1e9, 60058, 145 + 601); + EXPECT_NEAR(get_mag_strength_tesla(-50, 100) * 1e9, 61633, 145 + 616); + EXPECT_NEAR(get_mag_strength_tesla(-50, 105) * 1e9, 62963, 145 + 630); + EXPECT_NEAR(get_mag_strength_tesla(-50, 110) * 1e9, 64035, 145 + 640); + EXPECT_NEAR(get_mag_strength_tesla(-50, 115) * 1e9, 64845, 145 + 648); + EXPECT_NEAR(get_mag_strength_tesla(-50, 120) * 1e9, 65399, 145 + 654); + EXPECT_NEAR(get_mag_strength_tesla(-50, 125) * 1e9, 65709, 145 + 657); + EXPECT_NEAR(get_mag_strength_tesla(-50, 130) * 1e9, 65791, 145 + 658); + EXPECT_NEAR(get_mag_strength_tesla(-50, 135) * 1e9, 65664, 145 + 657); + EXPECT_NEAR(get_mag_strength_tesla(-50, 140) * 1e9, 65349, 145 + 653); + EXPECT_NEAR(get_mag_strength_tesla(-50, 145) * 1e9, 64864, 145 + 649); + EXPECT_NEAR(get_mag_strength_tesla(-50, 150) * 1e9, 64231, 145 + 642); + EXPECT_NEAR(get_mag_strength_tesla(-50, 155) * 1e9, 63468, 145 + 635); + EXPECT_NEAR(get_mag_strength_tesla(-50, 160) * 1e9, 62596, 145 + 626); + EXPECT_NEAR(get_mag_strength_tesla(-50, 165) * 1e9, 61633, 145 + 616); + EXPECT_NEAR(get_mag_strength_tesla(-50, 170) * 1e9, 60598, 145 + 606); + EXPECT_NEAR(get_mag_strength_tesla(-50, 175) * 1e9, 59508, 145 + 595); + EXPECT_NEAR(get_mag_strength_tesla(-50, 180) * 1e9, 58381, 145 + 584); + EXPECT_NEAR(get_mag_strength_tesla(-45, -180) * 1e9, 56229, 145 + 956); + EXPECT_NEAR(get_mag_strength_tesla(-45, -175) * 1e9, 55022, 145 + 935); + EXPECT_NEAR(get_mag_strength_tesla(-45, -170) * 1e9, 53807, 145 + 915); + EXPECT_NEAR(get_mag_strength_tesla(-45, -165) * 1e9, 52592, 145 + 894); + EXPECT_NEAR(get_mag_strength_tesla(-45, -160) * 1e9, 51384, 145 + 874); + EXPECT_NEAR(get_mag_strength_tesla(-45, -155) * 1e9, 50188, 145 + 853); + EXPECT_NEAR(get_mag_strength_tesla(-45, -150) * 1e9, 49004, 145 + 833); + EXPECT_NEAR(get_mag_strength_tesla(-45, -145) * 1e9, 47828, 145 + 813); + EXPECT_NEAR(get_mag_strength_tesla(-45, -140) * 1e9, 46650, 145 + 793); + EXPECT_NEAR(get_mag_strength_tesla(-45, -135) * 1e9, 45453, 145 + 773); + EXPECT_NEAR(get_mag_strength_tesla(-45, -130) * 1e9, 44217, 145 + 752); + EXPECT_NEAR(get_mag_strength_tesla(-45, -125) * 1e9, 42923, 145 + 730); + EXPECT_NEAR(get_mag_strength_tesla(-45, -120) * 1e9, 41554, 145 + 706); + EXPECT_NEAR(get_mag_strength_tesla(-45, -115) * 1e9, 40100, 145 + 682); + EXPECT_NEAR(get_mag_strength_tesla(-45, -110) * 1e9, 38562, 145 + 656); + EXPECT_NEAR(get_mag_strength_tesla(-45, -105) * 1e9, 36949, 145 + 628); + EXPECT_NEAR(get_mag_strength_tesla(-45, -100) * 1e9, 35284, 145 + 600); + EXPECT_NEAR(get_mag_strength_tesla(-45, -95) * 1e9, 33598, 145 + 571); + EXPECT_NEAR(get_mag_strength_tesla(-45, -90) * 1e9, 31937, 145 + 543); + EXPECT_NEAR(get_mag_strength_tesla(-45, -85) * 1e9, 30351, 145 + 516); + EXPECT_NEAR(get_mag_strength_tesla(-45, -80) * 1e9, 28896, 145 + 491); + EXPECT_NEAR(get_mag_strength_tesla(-45, -75) * 1e9, 27622, 145 + 470); + EXPECT_NEAR(get_mag_strength_tesla(-45, -70) * 1e9, 26568, 145 + 452); + EXPECT_NEAR(get_mag_strength_tesla(-45, -65) * 1e9, 25754, 145 + 438); + EXPECT_NEAR(get_mag_strength_tesla(-45, -60) * 1e9, 25177, 145 + 428); + EXPECT_NEAR(get_mag_strength_tesla(-45, -55) * 1e9, 24809, 145 + 422); + EXPECT_NEAR(get_mag_strength_tesla(-45, -50) * 1e9, 24603, 145 + 418); + EXPECT_NEAR(get_mag_strength_tesla(-45, -45) * 1e9, 24505, 145 + 417); + EXPECT_NEAR(get_mag_strength_tesla(-45, -40) * 1e9, 24462, 145 + 416); + EXPECT_NEAR(get_mag_strength_tesla(-45, -35) * 1e9, 24429, 145 + 415); + EXPECT_NEAR(get_mag_strength_tesla(-45, -30) * 1e9, 24379, 145 + 414); + EXPECT_NEAR(get_mag_strength_tesla(-45, -25) * 1e9, 24298, 145 + 413); + EXPECT_NEAR(get_mag_strength_tesla(-45, -20) * 1e9, 24185, 145 + 411); + EXPECT_NEAR(get_mag_strength_tesla(-45, -15) * 1e9, 24053, 145 + 409); + EXPECT_NEAR(get_mag_strength_tesla(-45, -10) * 1e9, 23926, 145 + 407); + EXPECT_NEAR(get_mag_strength_tesla(-45, -5) * 1e9, 23840, 145 + 405); + EXPECT_NEAR(get_mag_strength_tesla(-45, 0) * 1e9, 23846, 145 + 405); + EXPECT_NEAR(get_mag_strength_tesla(-45, 5) * 1e9, 24003, 145 + 408); + EXPECT_NEAR(get_mag_strength_tesla(-45, 10) * 1e9, 24375, 145 + 414); + EXPECT_NEAR(get_mag_strength_tesla(-45, 15) * 1e9, 25024, 145 + 425); + EXPECT_NEAR(get_mag_strength_tesla(-45, 20) * 1e9, 25994, 145 + 442); + EXPECT_NEAR(get_mag_strength_tesla(-45, 25) * 1e9, 27307, 145 + 464); + EXPECT_NEAR(get_mag_strength_tesla(-45, 30) * 1e9, 28961, 145 + 492); + EXPECT_NEAR(get_mag_strength_tesla(-45, 35) * 1e9, 30928, 145 + 526); + EXPECT_NEAR(get_mag_strength_tesla(-45, 40) * 1e9, 33162, 145 + 564); + EXPECT_NEAR(get_mag_strength_tesla(-45, 45) * 1e9, 35604, 145 + 605); + EXPECT_NEAR(get_mag_strength_tesla(-45, 50) * 1e9, 38189, 145 + 649); + EXPECT_NEAR(get_mag_strength_tesla(-45, 55) * 1e9, 40851, 145 + 694); + EXPECT_NEAR(get_mag_strength_tesla(-45, 60) * 1e9, 43529, 145 + 740); + EXPECT_NEAR(get_mag_strength_tesla(-45, 65) * 1e9, 46171, 145 + 785); + EXPECT_NEAR(get_mag_strength_tesla(-45, 70) * 1e9, 48733, 145 + 828); + EXPECT_NEAR(get_mag_strength_tesla(-45, 75) * 1e9, 51183, 145 + 870); + EXPECT_NEAR(get_mag_strength_tesla(-45, 80) * 1e9, 53492, 145 + 909); + EXPECT_NEAR(get_mag_strength_tesla(-45, 85) * 1e9, 55634, 145 + 946); + EXPECT_NEAR(get_mag_strength_tesla(-45, 90) * 1e9, 57582, 145 + 979); + EXPECT_NEAR(get_mag_strength_tesla(-45, 95) * 1e9, 59309, 145 + 1008); + EXPECT_NEAR(get_mag_strength_tesla(-45, 100) * 1e9, 60794, 145 + 1033); + EXPECT_NEAR(get_mag_strength_tesla(-45, 105) * 1e9, 62019, 145 + 1054); + EXPECT_NEAR(get_mag_strength_tesla(-45, 110) * 1e9, 62979, 145 + 1071); + EXPECT_NEAR(get_mag_strength_tesla(-45, 115) * 1e9, 63675, 145 + 1082); + EXPECT_NEAR(get_mag_strength_tesla(-45, 120) * 1e9, 64118, 145 + 1090); + EXPECT_NEAR(get_mag_strength_tesla(-45, 125) * 1e9, 64325, 145 + 1094); + EXPECT_NEAR(get_mag_strength_tesla(-45, 130) * 1e9, 64313, 145 + 1093); + EXPECT_NEAR(get_mag_strength_tesla(-45, 135) * 1e9, 64102, 145 + 1090); EXPECT_NEAR(get_mag_strength_tesla(-45, 140) * 1e9, 63711, 145 + 1083); - EXPECT_NEAR(get_mag_strength_tesla(-45, 145) * 1e9, 63157, 145 + 1074); - EXPECT_NEAR(get_mag_strength_tesla(-45, 150) * 1e9, 62459, 145 + 1062); - EXPECT_NEAR(get_mag_strength_tesla(-45, 155) * 1e9, 61632, 145 + 1048); - EXPECT_NEAR(get_mag_strength_tesla(-45, 160) * 1e9, 60696, 145 + 1032); - EXPECT_NEAR(get_mag_strength_tesla(-45, 165) * 1e9, 59668, 145 + 1014); - EXPECT_NEAR(get_mag_strength_tesla(-45, 170) * 1e9, 58570, 145 + 996); - EXPECT_NEAR(get_mag_strength_tesla(-45, 175) * 1e9, 57419, 145 + 976); - EXPECT_NEAR(get_mag_strength_tesla(-45, 180) * 1e9, 56234, 145 + 956); - EXPECT_NEAR(get_mag_strength_tesla(-40, -180) * 1e9, 53895, 145 + 916); - EXPECT_NEAR(get_mag_strength_tesla(-40, -175) * 1e9, 52662, 145 + 895); - EXPECT_NEAR(get_mag_strength_tesla(-40, -170) * 1e9, 51423, 145 + 874); - EXPECT_NEAR(get_mag_strength_tesla(-40, -165) * 1e9, 50186, 145 + 853); - EXPECT_NEAR(get_mag_strength_tesla(-40, -160) * 1e9, 48958, 145 + 832); - EXPECT_NEAR(get_mag_strength_tesla(-40, -155) * 1e9, 47745, 145 + 812); - EXPECT_NEAR(get_mag_strength_tesla(-40, -150) * 1e9, 46548, 145 + 791); - EXPECT_NEAR(get_mag_strength_tesla(-40, -145) * 1e9, 45368, 145 + 771); - EXPECT_NEAR(get_mag_strength_tesla(-40, -140) * 1e9, 44196, 145 + 751); - EXPECT_NEAR(get_mag_strength_tesla(-40, -135) * 1e9, 43018, 145 + 731); - EXPECT_NEAR(get_mag_strength_tesla(-40, -130) * 1e9, 41817, 145 + 711); - EXPECT_NEAR(get_mag_strength_tesla(-40, -125) * 1e9, 40572, 145 + 690); - EXPECT_NEAR(get_mag_strength_tesla(-40, -120) * 1e9, 39266, 145 + 668); - EXPECT_NEAR(get_mag_strength_tesla(-40, -115) * 1e9, 37884, 145 + 644); - EXPECT_NEAR(get_mag_strength_tesla(-40, -110) * 1e9, 36422, 145 + 619); - EXPECT_NEAR(get_mag_strength_tesla(-40, -105) * 1e9, 34883, 145 + 593); - EXPECT_NEAR(get_mag_strength_tesla(-40, -100) * 1e9, 33282, 145 + 566); - EXPECT_NEAR(get_mag_strength_tesla(-40, -95) * 1e9, 31648, 145 + 538); - EXPECT_NEAR(get_mag_strength_tesla(-40, -90) * 1e9, 30026, 145 + 510); - EXPECT_NEAR(get_mag_strength_tesla(-40, -85) * 1e9, 28471, 145 + 484); - EXPECT_NEAR(get_mag_strength_tesla(-40, -80) * 1e9, 27050, 145 + 460); - EXPECT_NEAR(get_mag_strength_tesla(-40, -75) * 1e9, 25823, 145 + 439); - EXPECT_NEAR(get_mag_strength_tesla(-40, -70) * 1e9, 24842, 145 + 422); - EXPECT_NEAR(get_mag_strength_tesla(-40, -65) * 1e9, 24133, 145 + 410); - EXPECT_NEAR(get_mag_strength_tesla(-40, -60) * 1e9, 23691, 145 + 403); - EXPECT_NEAR(get_mag_strength_tesla(-40, -55) * 1e9, 23482, 145 + 399); - EXPECT_NEAR(get_mag_strength_tesla(-40, -50) * 1e9, 23446, 145 + 399); - EXPECT_NEAR(get_mag_strength_tesla(-40, -45) * 1e9, 23518, 145 + 400); - EXPECT_NEAR(get_mag_strength_tesla(-40, -40) * 1e9, 23635, 145 + 402); - EXPECT_NEAR(get_mag_strength_tesla(-40, -35) * 1e9, 23750, 145 + 404); - EXPECT_NEAR(get_mag_strength_tesla(-40, -30) * 1e9, 23833, 145 + 405); - EXPECT_NEAR(get_mag_strength_tesla(-40, -25) * 1e9, 23871, 145 + 406); - EXPECT_NEAR(get_mag_strength_tesla(-40, -20) * 1e9, 23859, 145 + 406); - EXPECT_NEAR(get_mag_strength_tesla(-40, -15) * 1e9, 23804, 145 + 405); - EXPECT_NEAR(get_mag_strength_tesla(-40, -10) * 1e9, 23718, 145 + 403); - EXPECT_NEAR(get_mag_strength_tesla(-40, -5) * 1e9, 23629, 145 + 402); - EXPECT_NEAR(get_mag_strength_tesla(-40, 0) * 1e9, 23582, 145 + 401); - EXPECT_NEAR(get_mag_strength_tesla(-40, 5) * 1e9, 23641, 145 + 402); - EXPECT_NEAR(get_mag_strength_tesla(-40, 10) * 1e9, 23883, 145 + 406); - EXPECT_NEAR(get_mag_strength_tesla(-40, 15) * 1e9, 24391, 145 + 415); - EXPECT_NEAR(get_mag_strength_tesla(-40, 20) * 1e9, 25233, 145 + 429); - EXPECT_NEAR(get_mag_strength_tesla(-40, 25) * 1e9, 26450, 145 + 450); - EXPECT_NEAR(get_mag_strength_tesla(-40, 30) * 1e9, 28053, 145 + 477); - EXPECT_NEAR(get_mag_strength_tesla(-40, 35) * 1e9, 30017, 145 + 510); - EXPECT_NEAR(get_mag_strength_tesla(-40, 40) * 1e9, 32290, 145 + 549); - EXPECT_NEAR(get_mag_strength_tesla(-40, 45) * 1e9, 34801, 145 + 592); - EXPECT_NEAR(get_mag_strength_tesla(-40, 50) * 1e9, 37468, 145 + 637); - EXPECT_NEAR(get_mag_strength_tesla(-40, 55) * 1e9, 40209, 145 + 684); - EXPECT_NEAR(get_mag_strength_tesla(-40, 60) * 1e9, 42946, 145 + 730); - EXPECT_NEAR(get_mag_strength_tesla(-40, 65) * 1e9, 45618, 145 + 776); - EXPECT_NEAR(get_mag_strength_tesla(-40, 70) * 1e9, 48176, 145 + 819); - EXPECT_NEAR(get_mag_strength_tesla(-40, 75) * 1e9, 50585, 145 + 860); - EXPECT_NEAR(get_mag_strength_tesla(-40, 80) * 1e9, 52821, 145 + 898); - EXPECT_NEAR(get_mag_strength_tesla(-40, 85) * 1e9, 54861, 145 + 933); - EXPECT_NEAR(get_mag_strength_tesla(-40, 90) * 1e9, 56683, 145 + 964); - EXPECT_NEAR(get_mag_strength_tesla(-40, 95) * 1e9, 58266, 145 + 991); - EXPECT_NEAR(get_mag_strength_tesla(-40, 100) * 1e9, 59592, 145 + 1013); - EXPECT_NEAR(get_mag_strength_tesla(-40, 105) * 1e9, 60655, 145 + 1031); - EXPECT_NEAR(get_mag_strength_tesla(-40, 110) * 1e9, 61456, 145 + 1045); - EXPECT_NEAR(get_mag_strength_tesla(-40, 115) * 1e9, 62007, 145 + 1054); - EXPECT_NEAR(get_mag_strength_tesla(-40, 120) * 1e9, 62326, 145 + 1060); - EXPECT_NEAR(get_mag_strength_tesla(-40, 125) * 1e9, 62432, 145 + 1061); - EXPECT_NEAR(get_mag_strength_tesla(-40, 130) * 1e9, 62342, 145 + 1060); - EXPECT_NEAR(get_mag_strength_tesla(-40, 135) * 1e9, 62072, 145 + 1055); - EXPECT_NEAR(get_mag_strength_tesla(-40, 140) * 1e9, 61635, 145 + 1048); - EXPECT_NEAR(get_mag_strength_tesla(-40, 145) * 1e9, 61044, 145 + 1038); + EXPECT_NEAR(get_mag_strength_tesla(-45, 145) * 1e9, 63158, 145 + 1074); + EXPECT_NEAR(get_mag_strength_tesla(-45, 150) * 1e9, 62458, 145 + 1062); + EXPECT_NEAR(get_mag_strength_tesla(-45, 155) * 1e9, 61631, 145 + 1048); + EXPECT_NEAR(get_mag_strength_tesla(-45, 160) * 1e9, 60693, 145 + 1032); + EXPECT_NEAR(get_mag_strength_tesla(-45, 165) * 1e9, 59665, 145 + 1014); + EXPECT_NEAR(get_mag_strength_tesla(-45, 170) * 1e9, 58566, 145 + 996); + EXPECT_NEAR(get_mag_strength_tesla(-45, 175) * 1e9, 57415, 145 + 976); + EXPECT_NEAR(get_mag_strength_tesla(-45, 180) * 1e9, 56229, 145 + 956); + EXPECT_NEAR(get_mag_strength_tesla(-40, -180) * 1e9, 53891, 145 + 916); + EXPECT_NEAR(get_mag_strength_tesla(-40, -175) * 1e9, 52657, 145 + 895); + EXPECT_NEAR(get_mag_strength_tesla(-40, -170) * 1e9, 51417, 145 + 874); + EXPECT_NEAR(get_mag_strength_tesla(-40, -165) * 1e9, 50180, 145 + 853); + EXPECT_NEAR(get_mag_strength_tesla(-40, -160) * 1e9, 48951, 145 + 832); + EXPECT_NEAR(get_mag_strength_tesla(-40, -155) * 1e9, 47737, 145 + 812); + EXPECT_NEAR(get_mag_strength_tesla(-40, -150) * 1e9, 46540, 145 + 791); + EXPECT_NEAR(get_mag_strength_tesla(-40, -145) * 1e9, 45359, 145 + 771); + EXPECT_NEAR(get_mag_strength_tesla(-40, -140) * 1e9, 44187, 145 + 751); + EXPECT_NEAR(get_mag_strength_tesla(-40, -135) * 1e9, 43009, 145 + 731); + EXPECT_NEAR(get_mag_strength_tesla(-40, -130) * 1e9, 41807, 145 + 711); + EXPECT_NEAR(get_mag_strength_tesla(-40, -125) * 1e9, 40562, 145 + 690); + EXPECT_NEAR(get_mag_strength_tesla(-40, -120) * 1e9, 39255, 145 + 667); + EXPECT_NEAR(get_mag_strength_tesla(-40, -115) * 1e9, 37873, 145 + 644); + EXPECT_NEAR(get_mag_strength_tesla(-40, -110) * 1e9, 36411, 145 + 619); + EXPECT_NEAR(get_mag_strength_tesla(-40, -105) * 1e9, 34872, 145 + 593); + EXPECT_NEAR(get_mag_strength_tesla(-40, -100) * 1e9, 33271, 145 + 566); + EXPECT_NEAR(get_mag_strength_tesla(-40, -95) * 1e9, 31637, 145 + 538); + EXPECT_NEAR(get_mag_strength_tesla(-40, -90) * 1e9, 30015, 145 + 510); + EXPECT_NEAR(get_mag_strength_tesla(-40, -85) * 1e9, 28461, 145 + 484); + EXPECT_NEAR(get_mag_strength_tesla(-40, -80) * 1e9, 27040, 145 + 460); + EXPECT_NEAR(get_mag_strength_tesla(-40, -75) * 1e9, 25814, 145 + 439); + EXPECT_NEAR(get_mag_strength_tesla(-40, -70) * 1e9, 24834, 145 + 422); + EXPECT_NEAR(get_mag_strength_tesla(-40, -65) * 1e9, 24126, 145 + 410); + EXPECT_NEAR(get_mag_strength_tesla(-40, -60) * 1e9, 23686, 145 + 403); + EXPECT_NEAR(get_mag_strength_tesla(-40, -55) * 1e9, 23477, 145 + 399); + EXPECT_NEAR(get_mag_strength_tesla(-40, -50) * 1e9, 23442, 145 + 399); + EXPECT_NEAR(get_mag_strength_tesla(-40, -45) * 1e9, 23514, 145 + 400); + EXPECT_NEAR(get_mag_strength_tesla(-40, -40) * 1e9, 23631, 145 + 402); + EXPECT_NEAR(get_mag_strength_tesla(-40, -35) * 1e9, 23745, 145 + 404); + EXPECT_NEAR(get_mag_strength_tesla(-40, -30) * 1e9, 23827, 145 + 405); + EXPECT_NEAR(get_mag_strength_tesla(-40, -25) * 1e9, 23864, 145 + 406); + EXPECT_NEAR(get_mag_strength_tesla(-40, -20) * 1e9, 23851, 145 + 405); + EXPECT_NEAR(get_mag_strength_tesla(-40, -15) * 1e9, 23794, 145 + 405); + EXPECT_NEAR(get_mag_strength_tesla(-40, -10) * 1e9, 23708, 145 + 403); + EXPECT_NEAR(get_mag_strength_tesla(-40, -5) * 1e9, 23618, 145 + 402); + EXPECT_NEAR(get_mag_strength_tesla(-40, 0) * 1e9, 23571, 145 + 401); + EXPECT_NEAR(get_mag_strength_tesla(-40, 5) * 1e9, 23630, 145 + 402); + EXPECT_NEAR(get_mag_strength_tesla(-40, 10) * 1e9, 23874, 145 + 406); + EXPECT_NEAR(get_mag_strength_tesla(-40, 15) * 1e9, 24383, 145 + 415); + EXPECT_NEAR(get_mag_strength_tesla(-40, 20) * 1e9, 25228, 145 + 429); + EXPECT_NEAR(get_mag_strength_tesla(-40, 25) * 1e9, 26449, 145 + 450); + EXPECT_NEAR(get_mag_strength_tesla(-40, 30) * 1e9, 28055, 145 + 477); + EXPECT_NEAR(get_mag_strength_tesla(-40, 35) * 1e9, 30022, 145 + 510); + EXPECT_NEAR(get_mag_strength_tesla(-40, 40) * 1e9, 32298, 145 + 549); + EXPECT_NEAR(get_mag_strength_tesla(-40, 45) * 1e9, 34811, 145 + 592); + EXPECT_NEAR(get_mag_strength_tesla(-40, 50) * 1e9, 37480, 145 + 637); + EXPECT_NEAR(get_mag_strength_tesla(-40, 55) * 1e9, 40222, 145 + 684); + EXPECT_NEAR(get_mag_strength_tesla(-40, 60) * 1e9, 42960, 145 + 730); + EXPECT_NEAR(get_mag_strength_tesla(-40, 65) * 1e9, 45631, 145 + 776); + EXPECT_NEAR(get_mag_strength_tesla(-40, 70) * 1e9, 48187, 145 + 819); + EXPECT_NEAR(get_mag_strength_tesla(-40, 75) * 1e9, 50596, 145 + 860); + EXPECT_NEAR(get_mag_strength_tesla(-40, 80) * 1e9, 52831, 145 + 898); + EXPECT_NEAR(get_mag_strength_tesla(-40, 85) * 1e9, 54870, 145 + 933); + EXPECT_NEAR(get_mag_strength_tesla(-40, 90) * 1e9, 56691, 145 + 964); + EXPECT_NEAR(get_mag_strength_tesla(-40, 95) * 1e9, 58272, 145 + 991); + EXPECT_NEAR(get_mag_strength_tesla(-40, 100) * 1e9, 59598, 145 + 1013); + EXPECT_NEAR(get_mag_strength_tesla(-40, 105) * 1e9, 60660, 145 + 1031); + EXPECT_NEAR(get_mag_strength_tesla(-40, 110) * 1e9, 61461, 145 + 1045); + EXPECT_NEAR(get_mag_strength_tesla(-40, 115) * 1e9, 62012, 145 + 1054); + EXPECT_NEAR(get_mag_strength_tesla(-40, 120) * 1e9, 62330, 145 + 1060); + EXPECT_NEAR(get_mag_strength_tesla(-40, 125) * 1e9, 62435, 145 + 1061); + EXPECT_NEAR(get_mag_strength_tesla(-40, 130) * 1e9, 62344, 145 + 1060); + EXPECT_NEAR(get_mag_strength_tesla(-40, 135) * 1e9, 62073, 145 + 1055); + EXPECT_NEAR(get_mag_strength_tesla(-40, 140) * 1e9, 61636, 145 + 1048); + EXPECT_NEAR(get_mag_strength_tesla(-40, 145) * 1e9, 61045, 145 + 1038); EXPECT_NEAR(get_mag_strength_tesla(-40, 150) * 1e9, 60313, 145 + 1025); - EXPECT_NEAR(get_mag_strength_tesla(-40, 155) * 1e9, 59456, 145 + 1011); - EXPECT_NEAR(get_mag_strength_tesla(-40, 160) * 1e9, 58488, 145 + 994); - EXPECT_NEAR(get_mag_strength_tesla(-40, 165) * 1e9, 57427, 145 + 976); - EXPECT_NEAR(get_mag_strength_tesla(-40, 170) * 1e9, 56295, 145 + 957); - EXPECT_NEAR(get_mag_strength_tesla(-40, 175) * 1e9, 55111, 145 + 937); - EXPECT_NEAR(get_mag_strength_tesla(-40, 180) * 1e9, 53895, 145 + 916); - EXPECT_NEAR(get_mag_strength_tesla(-35, -180) * 1e9, 51398, 145 + 874); - EXPECT_NEAR(get_mag_strength_tesla(-35, -175) * 1e9, 50163, 145 + 853); - EXPECT_NEAR(get_mag_strength_tesla(-35, -170) * 1e9, 48924, 145 + 832); - EXPECT_NEAR(get_mag_strength_tesla(-35, -165) * 1e9, 47690, 145 + 811); - EXPECT_NEAR(get_mag_strength_tesla(-35, -160) * 1e9, 46464, 145 + 790); - EXPECT_NEAR(get_mag_strength_tesla(-35, -155) * 1e9, 45251, 145 + 769); - EXPECT_NEAR(get_mag_strength_tesla(-35, -150) * 1e9, 44057, 145 + 749); - EXPECT_NEAR(get_mag_strength_tesla(-35, -145) * 1e9, 42884, 145 + 729); - EXPECT_NEAR(get_mag_strength_tesla(-35, -140) * 1e9, 41728, 145 + 709); - EXPECT_NEAR(get_mag_strength_tesla(-35, -135) * 1e9, 40580, 145 + 690); - EXPECT_NEAR(get_mag_strength_tesla(-35, -130) * 1e9, 39426, 145 + 670); - EXPECT_NEAR(get_mag_strength_tesla(-35, -125) * 1e9, 38250, 145 + 650); - EXPECT_NEAR(get_mag_strength_tesla(-35, -120) * 1e9, 37035, 145 + 630); - EXPECT_NEAR(get_mag_strength_tesla(-35, -115) * 1e9, 35764, 145 + 608); - EXPECT_NEAR(get_mag_strength_tesla(-35, -110) * 1e9, 34425, 145 + 585); - EXPECT_NEAR(get_mag_strength_tesla(-35, -105) * 1e9, 33014, 145 + 561); - EXPECT_NEAR(get_mag_strength_tesla(-35, -100) * 1e9, 31535, 145 + 536); - EXPECT_NEAR(get_mag_strength_tesla(-35, -95) * 1e9, 30011, 145 + 510); - EXPECT_NEAR(get_mag_strength_tesla(-35, -90) * 1e9, 28483, 145 + 484); - EXPECT_NEAR(get_mag_strength_tesla(-35, -85) * 1e9, 27011, 145 + 459); - EXPECT_NEAR(get_mag_strength_tesla(-35, -80) * 1e9, 25667, 145 + 436); - EXPECT_NEAR(get_mag_strength_tesla(-35, -75) * 1e9, 24524, 145 + 417); - EXPECT_NEAR(get_mag_strength_tesla(-35, -70) * 1e9, 23640, 145 + 402); - EXPECT_NEAR(get_mag_strength_tesla(-35, -65) * 1e9, 23047, 145 + 392); - EXPECT_NEAR(get_mag_strength_tesla(-35, -60) * 1e9, 22738, 145 + 387); - EXPECT_NEAR(get_mag_strength_tesla(-35, -55) * 1e9, 22673, 145 + 385); - EXPECT_NEAR(get_mag_strength_tesla(-35, -50) * 1e9, 22783, 145 + 387); - EXPECT_NEAR(get_mag_strength_tesla(-35, -45) * 1e9, 22995, 145 + 391); - EXPECT_NEAR(get_mag_strength_tesla(-35, -40) * 1e9, 23246, 145 + 395); - EXPECT_NEAR(get_mag_strength_tesla(-35, -35) * 1e9, 23492, 145 + 399); - EXPECT_NEAR(get_mag_strength_tesla(-35, -30) * 1e9, 23708, 145 + 403); - EXPECT_NEAR(get_mag_strength_tesla(-35, -25) * 1e9, 23884, 145 + 406); - EXPECT_NEAR(get_mag_strength_tesla(-35, -20) * 1e9, 24016, 145 + 408); - EXPECT_NEAR(get_mag_strength_tesla(-35, -15) * 1e9, 24097, 145 + 410); - EXPECT_NEAR(get_mag_strength_tesla(-35, -10) * 1e9, 24125, 145 + 410); - EXPECT_NEAR(get_mag_strength_tesla(-35, -5) * 1e9, 24108, 145 + 410); - EXPECT_NEAR(get_mag_strength_tesla(-35, 0) * 1e9, 24076, 145 + 409); - EXPECT_NEAR(get_mag_strength_tesla(-35, 5) * 1e9, 24087, 145 + 409); - EXPECT_NEAR(get_mag_strength_tesla(-35, 10) * 1e9, 24224, 145 + 412); - EXPECT_NEAR(get_mag_strength_tesla(-35, 15) * 1e9, 24582, 145 + 418); - EXPECT_NEAR(get_mag_strength_tesla(-35, 20) * 1e9, 25252, 145 + 429); - EXPECT_NEAR(get_mag_strength_tesla(-35, 25) * 1e9, 26303, 145 + 447); - EXPECT_NEAR(get_mag_strength_tesla(-35, 30) * 1e9, 27766, 145 + 472); - EXPECT_NEAR(get_mag_strength_tesla(-35, 35) * 1e9, 29629, 145 + 504); - EXPECT_NEAR(get_mag_strength_tesla(-35, 40) * 1e9, 31844, 145 + 541); - EXPECT_NEAR(get_mag_strength_tesla(-35, 45) * 1e9, 34334, 145 + 584); - EXPECT_NEAR(get_mag_strength_tesla(-35, 50) * 1e9, 37002, 145 + 629); - EXPECT_NEAR(get_mag_strength_tesla(-35, 55) * 1e9, 39750, 145 + 676); - EXPECT_NEAR(get_mag_strength_tesla(-35, 60) * 1e9, 42484, 145 + 722); - EXPECT_NEAR(get_mag_strength_tesla(-35, 65) * 1e9, 45129, 145 + 767); - EXPECT_NEAR(get_mag_strength_tesla(-35, 70) * 1e9, 47628, 145 + 810); - EXPECT_NEAR(get_mag_strength_tesla(-35, 75) * 1e9, 49947, 145 + 849); - EXPECT_NEAR(get_mag_strength_tesla(-35, 80) * 1e9, 52060, 145 + 885); - EXPECT_NEAR(get_mag_strength_tesla(-35, 85) * 1e9, 53946, 145 + 917); - EXPECT_NEAR(get_mag_strength_tesla(-35, 90) * 1e9, 55588, 145 + 945); - EXPECT_NEAR(get_mag_strength_tesla(-35, 95) * 1e9, 56969, 145 + 968); - EXPECT_NEAR(get_mag_strength_tesla(-35, 100) * 1e9, 58082, 145 + 987); - EXPECT_NEAR(get_mag_strength_tesla(-35, 105) * 1e9, 58933, 145 + 1002); - EXPECT_NEAR(get_mag_strength_tesla(-35, 110) * 1e9, 59537, 145 + 1012); - EXPECT_NEAR(get_mag_strength_tesla(-35, 115) * 1e9, 59920, 145 + 1019); - EXPECT_NEAR(get_mag_strength_tesla(-35, 120) * 1e9, 60107, 145 + 1022); - EXPECT_NEAR(get_mag_strength_tesla(-35, 125) * 1e9, 60117, 145 + 1022); - EXPECT_NEAR(get_mag_strength_tesla(-35, 130) * 1e9, 59964, 145 + 1019); - EXPECT_NEAR(get_mag_strength_tesla(-35, 135) * 1e9, 59654, 145 + 1014); - EXPECT_NEAR(get_mag_strength_tesla(-35, 140) * 1e9, 59194, 145 + 1006); + EXPECT_NEAR(get_mag_strength_tesla(-40, 155) * 1e9, 59455, 145 + 1011); + EXPECT_NEAR(get_mag_strength_tesla(-40, 160) * 1e9, 58486, 145 + 994); + EXPECT_NEAR(get_mag_strength_tesla(-40, 165) * 1e9, 57425, 145 + 976); + EXPECT_NEAR(get_mag_strength_tesla(-40, 170) * 1e9, 56292, 145 + 957); + EXPECT_NEAR(get_mag_strength_tesla(-40, 175) * 1e9, 55108, 145 + 937); + EXPECT_NEAR(get_mag_strength_tesla(-40, 180) * 1e9, 53891, 145 + 916); + EXPECT_NEAR(get_mag_strength_tesla(-35, -180) * 1e9, 51395, 145 + 874); + EXPECT_NEAR(get_mag_strength_tesla(-35, -175) * 1e9, 50159, 145 + 853); + EXPECT_NEAR(get_mag_strength_tesla(-35, -170) * 1e9, 48920, 145 + 832); + EXPECT_NEAR(get_mag_strength_tesla(-35, -165) * 1e9, 47684, 145 + 811); + EXPECT_NEAR(get_mag_strength_tesla(-35, -160) * 1e9, 46457, 145 + 790); + EXPECT_NEAR(get_mag_strength_tesla(-35, -155) * 1e9, 45244, 145 + 769); + EXPECT_NEAR(get_mag_strength_tesla(-35, -150) * 1e9, 44050, 145 + 749); + EXPECT_NEAR(get_mag_strength_tesla(-35, -145) * 1e9, 42876, 145 + 729); + EXPECT_NEAR(get_mag_strength_tesla(-35, -140) * 1e9, 41719, 145 + 709); + EXPECT_NEAR(get_mag_strength_tesla(-35, -135) * 1e9, 40571, 145 + 690); + EXPECT_NEAR(get_mag_strength_tesla(-35, -130) * 1e9, 39417, 145 + 670); + EXPECT_NEAR(get_mag_strength_tesla(-35, -125) * 1e9, 38241, 145 + 650); + EXPECT_NEAR(get_mag_strength_tesla(-35, -120) * 1e9, 37025, 145 + 629); + EXPECT_NEAR(get_mag_strength_tesla(-35, -115) * 1e9, 35754, 145 + 608); + EXPECT_NEAR(get_mag_strength_tesla(-35, -110) * 1e9, 34415, 145 + 585); + EXPECT_NEAR(get_mag_strength_tesla(-35, -105) * 1e9, 33004, 145 + 561); + EXPECT_NEAR(get_mag_strength_tesla(-35, -100) * 1e9, 31525, 145 + 536); + EXPECT_NEAR(get_mag_strength_tesla(-35, -95) * 1e9, 30000, 145 + 510); + EXPECT_NEAR(get_mag_strength_tesla(-35, -90) * 1e9, 28473, 145 + 484); + EXPECT_NEAR(get_mag_strength_tesla(-35, -85) * 1e9, 27001, 145 + 459); + EXPECT_NEAR(get_mag_strength_tesla(-35, -80) * 1e9, 25658, 145 + 436); + EXPECT_NEAR(get_mag_strength_tesla(-35, -75) * 1e9, 24516, 145 + 417); + EXPECT_NEAR(get_mag_strength_tesla(-35, -70) * 1e9, 23633, 145 + 402); + EXPECT_NEAR(get_mag_strength_tesla(-35, -65) * 1e9, 23041, 145 + 392); + EXPECT_NEAR(get_mag_strength_tesla(-35, -60) * 1e9, 22734, 145 + 386); + EXPECT_NEAR(get_mag_strength_tesla(-35, -55) * 1e9, 22669, 145 + 385); + EXPECT_NEAR(get_mag_strength_tesla(-35, -50) * 1e9, 22780, 145 + 387); + EXPECT_NEAR(get_mag_strength_tesla(-35, -45) * 1e9, 22992, 145 + 391); + EXPECT_NEAR(get_mag_strength_tesla(-35, -40) * 1e9, 23243, 145 + 395); + EXPECT_NEAR(get_mag_strength_tesla(-35, -35) * 1e9, 23488, 145 + 399); + EXPECT_NEAR(get_mag_strength_tesla(-35, -30) * 1e9, 23703, 145 + 403); + EXPECT_NEAR(get_mag_strength_tesla(-35, -25) * 1e9, 23879, 145 + 406); + EXPECT_NEAR(get_mag_strength_tesla(-35, -20) * 1e9, 24009, 145 + 408); + EXPECT_NEAR(get_mag_strength_tesla(-35, -15) * 1e9, 24089, 145 + 410); + EXPECT_NEAR(get_mag_strength_tesla(-35, -10) * 1e9, 24115, 145 + 410); + EXPECT_NEAR(get_mag_strength_tesla(-35, -5) * 1e9, 24097, 145 + 410); + EXPECT_NEAR(get_mag_strength_tesla(-35, 0) * 1e9, 24065, 145 + 409); + EXPECT_NEAR(get_mag_strength_tesla(-35, 5) * 1e9, 24076, 145 + 409); + EXPECT_NEAR(get_mag_strength_tesla(-35, 10) * 1e9, 24213, 145 + 412); + EXPECT_NEAR(get_mag_strength_tesla(-35, 15) * 1e9, 24572, 145 + 418); + EXPECT_NEAR(get_mag_strength_tesla(-35, 20) * 1e9, 25246, 145 + 429); + EXPECT_NEAR(get_mag_strength_tesla(-35, 25) * 1e9, 26301, 145 + 447); + EXPECT_NEAR(get_mag_strength_tesla(-35, 30) * 1e9, 27767, 145 + 472); + EXPECT_NEAR(get_mag_strength_tesla(-35, 35) * 1e9, 29634, 145 + 504); + EXPECT_NEAR(get_mag_strength_tesla(-35, 40) * 1e9, 31853, 145 + 541); + EXPECT_NEAR(get_mag_strength_tesla(-35, 45) * 1e9, 34345, 145 + 584); + EXPECT_NEAR(get_mag_strength_tesla(-35, 50) * 1e9, 37016, 145 + 629); + EXPECT_NEAR(get_mag_strength_tesla(-35, 55) * 1e9, 39765, 145 + 676); + EXPECT_NEAR(get_mag_strength_tesla(-35, 60) * 1e9, 42499, 145 + 722); + EXPECT_NEAR(get_mag_strength_tesla(-35, 65) * 1e9, 45143, 145 + 767); + EXPECT_NEAR(get_mag_strength_tesla(-35, 70) * 1e9, 47642, 145 + 810); + EXPECT_NEAR(get_mag_strength_tesla(-35, 75) * 1e9, 49959, 145 + 849); + EXPECT_NEAR(get_mag_strength_tesla(-35, 80) * 1e9, 52070, 145 + 885); + EXPECT_NEAR(get_mag_strength_tesla(-35, 85) * 1e9, 53955, 145 + 917); + EXPECT_NEAR(get_mag_strength_tesla(-35, 90) * 1e9, 55595, 145 + 945); + EXPECT_NEAR(get_mag_strength_tesla(-35, 95) * 1e9, 56975, 145 + 969); + EXPECT_NEAR(get_mag_strength_tesla(-35, 100) * 1e9, 58088, 145 + 987); + EXPECT_NEAR(get_mag_strength_tesla(-35, 105) * 1e9, 58937, 145 + 1002); + EXPECT_NEAR(get_mag_strength_tesla(-35, 110) * 1e9, 59541, 145 + 1012); + EXPECT_NEAR(get_mag_strength_tesla(-35, 115) * 1e9, 59924, 145 + 1019); + EXPECT_NEAR(get_mag_strength_tesla(-35, 120) * 1e9, 60110, 145 + 1022); + EXPECT_NEAR(get_mag_strength_tesla(-35, 125) * 1e9, 60120, 145 + 1022); + EXPECT_NEAR(get_mag_strength_tesla(-35, 130) * 1e9, 59966, 145 + 1019); + EXPECT_NEAR(get_mag_strength_tesla(-35, 135) * 1e9, 59656, 145 + 1014); + EXPECT_NEAR(get_mag_strength_tesla(-35, 140) * 1e9, 59195, 145 + 1006); EXPECT_NEAR(get_mag_strength_tesla(-35, 145) * 1e9, 58591, 145 + 996); EXPECT_NEAR(get_mag_strength_tesla(-35, 150) * 1e9, 57852, 145 + 983); EXPECT_NEAR(get_mag_strength_tesla(-35, 155) * 1e9, 56990, 145 + 969); - EXPECT_NEAR(get_mag_strength_tesla(-35, 160) * 1e9, 56017, 145 + 952); - EXPECT_NEAR(get_mag_strength_tesla(-35, 165) * 1e9, 54951, 145 + 934); - EXPECT_NEAR(get_mag_strength_tesla(-35, 170) * 1e9, 53811, 145 + 915); - EXPECT_NEAR(get_mag_strength_tesla(-35, 175) * 1e9, 52620, 145 + 895); - EXPECT_NEAR(get_mag_strength_tesla(-35, 180) * 1e9, 51398, 145 + 874); - EXPECT_NEAR(get_mag_strength_tesla(-30, -180) * 1e9, 48759, 145 + 488); - EXPECT_NEAR(get_mag_strength_tesla(-30, -175) * 1e9, 47548, 145 + 475); - EXPECT_NEAR(get_mag_strength_tesla(-30, -170) * 1e9, 46337, 145 + 463); - EXPECT_NEAR(get_mag_strength_tesla(-30, -165) * 1e9, 45131, 145 + 451); - EXPECT_NEAR(get_mag_strength_tesla(-30, -160) * 1e9, 43933, 145 + 439); - EXPECT_NEAR(get_mag_strength_tesla(-30, -155) * 1e9, 42746, 145 + 427); - EXPECT_NEAR(get_mag_strength_tesla(-30, -150) * 1e9, 41577, 145 + 416); - EXPECT_NEAR(get_mag_strength_tesla(-30, -145) * 1e9, 40430, 145 + 404); - EXPECT_NEAR(get_mag_strength_tesla(-30, -140) * 1e9, 39307, 145 + 393); - EXPECT_NEAR(get_mag_strength_tesla(-30, -135) * 1e9, 38204, 145 + 382); - EXPECT_NEAR(get_mag_strength_tesla(-30, -130) * 1e9, 37114, 145 + 371); - EXPECT_NEAR(get_mag_strength_tesla(-30, -125) * 1e9, 36024, 145 + 360); - EXPECT_NEAR(get_mag_strength_tesla(-30, -120) * 1e9, 34921, 145 + 349); - EXPECT_NEAR(get_mag_strength_tesla(-30, -115) * 1e9, 33788, 145 + 338); - EXPECT_NEAR(get_mag_strength_tesla(-30, -110) * 1e9, 32606, 145 + 326); - EXPECT_NEAR(get_mag_strength_tesla(-30, -105) * 1e9, 31360, 145 + 314); - EXPECT_NEAR(get_mag_strength_tesla(-30, -100) * 1e9, 30046, 145 + 300); - EXPECT_NEAR(get_mag_strength_tesla(-30, -95) * 1e9, 28676, 145 + 287); - EXPECT_NEAR(get_mag_strength_tesla(-30, -90) * 1e9, 27289, 145 + 273); - EXPECT_NEAR(get_mag_strength_tesla(-30, -85) * 1e9, 25944, 145 + 259); - EXPECT_NEAR(get_mag_strength_tesla(-30, -80) * 1e9, 24717, 145 + 247); - EXPECT_NEAR(get_mag_strength_tesla(-30, -75) * 1e9, 23686, 145 + 237); - EXPECT_NEAR(get_mag_strength_tesla(-30, -70) * 1e9, 22914, 145 + 229); - EXPECT_NEAR(get_mag_strength_tesla(-30, -65) * 1e9, 22434, 145 + 224); - EXPECT_NEAR(get_mag_strength_tesla(-30, -60) * 1e9, 22240, 145 + 222); - EXPECT_NEAR(get_mag_strength_tesla(-30, -55) * 1e9, 22287, 145 + 223); - EXPECT_NEAR(get_mag_strength_tesla(-30, -50) * 1e9, 22505, 145 + 225); - EXPECT_NEAR(get_mag_strength_tesla(-30, -45) * 1e9, 22820, 145 + 228); - EXPECT_NEAR(get_mag_strength_tesla(-30, -40) * 1e9, 23174, 145 + 232); - EXPECT_NEAR(get_mag_strength_tesla(-30, -35) * 1e9, 23530, 145 + 235); - EXPECT_NEAR(get_mag_strength_tesla(-30, -30) * 1e9, 23876, 145 + 239); - EXPECT_NEAR(get_mag_strength_tesla(-30, -25) * 1e9, 24206, 145 + 242); - EXPECT_NEAR(get_mag_strength_tesla(-30, -20) * 1e9, 24513, 145 + 245); - EXPECT_NEAR(get_mag_strength_tesla(-30, -15) * 1e9, 24782, 145 + 248); - EXPECT_NEAR(get_mag_strength_tesla(-30, -10) * 1e9, 24988, 145 + 250); - EXPECT_NEAR(get_mag_strength_tesla(-30, -5) * 1e9, 25117, 145 + 251); - EXPECT_NEAR(get_mag_strength_tesla(-30, 0) * 1e9, 25177, 145 + 252); - EXPECT_NEAR(get_mag_strength_tesla(-30, 5) * 1e9, 25210, 145 + 252); - EXPECT_NEAR(get_mag_strength_tesla(-30, 10) * 1e9, 25291, 145 + 253); - EXPECT_NEAR(get_mag_strength_tesla(-30, 15) * 1e9, 25520, 145 + 255); - EXPECT_NEAR(get_mag_strength_tesla(-30, 20) * 1e9, 26004, 145 + 260); - EXPECT_NEAR(get_mag_strength_tesla(-30, 25) * 1e9, 26834, 145 + 268); - EXPECT_NEAR(get_mag_strength_tesla(-30, 30) * 1e9, 28070, 145 + 281); - EXPECT_NEAR(get_mag_strength_tesla(-30, 35) * 1e9, 29726, 145 + 297); - EXPECT_NEAR(get_mag_strength_tesla(-30, 40) * 1e9, 31771, 145 + 318); - EXPECT_NEAR(get_mag_strength_tesla(-30, 45) * 1e9, 34130, 145 + 341); - EXPECT_NEAR(get_mag_strength_tesla(-30, 50) * 1e9, 36702, 145 + 367); - EXPECT_NEAR(get_mag_strength_tesla(-30, 55) * 1e9, 39371, 145 + 394); - EXPECT_NEAR(get_mag_strength_tesla(-30, 60) * 1e9, 42029, 145 + 420); - EXPECT_NEAR(get_mag_strength_tesla(-30, 65) * 1e9, 44588, 145 + 446); - EXPECT_NEAR(get_mag_strength_tesla(-30, 70) * 1e9, 46983, 145 + 470); - EXPECT_NEAR(get_mag_strength_tesla(-30, 75) * 1e9, 49174, 145 + 492); - EXPECT_NEAR(get_mag_strength_tesla(-30, 80) * 1e9, 51131, 145 + 511); - EXPECT_NEAR(get_mag_strength_tesla(-30, 85) * 1e9, 52834, 145 + 528); - EXPECT_NEAR(get_mag_strength_tesla(-30, 90) * 1e9, 54264, 145 + 543); - EXPECT_NEAR(get_mag_strength_tesla(-30, 95) * 1e9, 55409, 145 + 554); - EXPECT_NEAR(get_mag_strength_tesla(-30, 100) * 1e9, 56273, 145 + 563); - EXPECT_NEAR(get_mag_strength_tesla(-30, 105) * 1e9, 56879, 145 + 569); - EXPECT_NEAR(get_mag_strength_tesla(-30, 110) * 1e9, 57263, 145 + 573); - EXPECT_NEAR(get_mag_strength_tesla(-30, 115) * 1e9, 57466, 145 + 575); - EXPECT_NEAR(get_mag_strength_tesla(-30, 120) * 1e9, 57521, 145 + 575); - EXPECT_NEAR(get_mag_strength_tesla(-30, 125) * 1e9, 57446, 145 + 574); - EXPECT_NEAR(get_mag_strength_tesla(-30, 130) * 1e9, 57244, 145 + 572); - EXPECT_NEAR(get_mag_strength_tesla(-30, 135) * 1e9, 56912, 145 + 569); - EXPECT_NEAR(get_mag_strength_tesla(-30, 140) * 1e9, 56444, 145 + 564); - EXPECT_NEAR(get_mag_strength_tesla(-30, 145) * 1e9, 55842, 145 + 558); - EXPECT_NEAR(get_mag_strength_tesla(-30, 150) * 1e9, 55112, 145 + 551); + EXPECT_NEAR(get_mag_strength_tesla(-35, 160) * 1e9, 56016, 145 + 952); + EXPECT_NEAR(get_mag_strength_tesla(-35, 165) * 1e9, 54949, 145 + 934); + EXPECT_NEAR(get_mag_strength_tesla(-35, 170) * 1e9, 53808, 145 + 915); + EXPECT_NEAR(get_mag_strength_tesla(-35, 175) * 1e9, 52617, 145 + 894); + EXPECT_NEAR(get_mag_strength_tesla(-35, 180) * 1e9, 51395, 145 + 874); + EXPECT_NEAR(get_mag_strength_tesla(-30, -180) * 1e9, 48756, 145 + 488); + EXPECT_NEAR(get_mag_strength_tesla(-30, -175) * 1e9, 47544, 145 + 475); + EXPECT_NEAR(get_mag_strength_tesla(-30, -170) * 1e9, 46333, 145 + 463); + EXPECT_NEAR(get_mag_strength_tesla(-30, -165) * 1e9, 45126, 145 + 451); + EXPECT_NEAR(get_mag_strength_tesla(-30, -160) * 1e9, 43927, 145 + 439); + EXPECT_NEAR(get_mag_strength_tesla(-30, -155) * 1e9, 42740, 145 + 427); + EXPECT_NEAR(get_mag_strength_tesla(-30, -150) * 1e9, 41570, 145 + 416); + EXPECT_NEAR(get_mag_strength_tesla(-30, -145) * 1e9, 40422, 145 + 404); + EXPECT_NEAR(get_mag_strength_tesla(-30, -140) * 1e9, 39299, 145 + 393); + EXPECT_NEAR(get_mag_strength_tesla(-30, -135) * 1e9, 38196, 145 + 382); + EXPECT_NEAR(get_mag_strength_tesla(-30, -130) * 1e9, 37106, 145 + 371); + EXPECT_NEAR(get_mag_strength_tesla(-30, -125) * 1e9, 36016, 145 + 360); + EXPECT_NEAR(get_mag_strength_tesla(-30, -120) * 1e9, 34912, 145 + 349); + EXPECT_NEAR(get_mag_strength_tesla(-30, -115) * 1e9, 33779, 145 + 338); + EXPECT_NEAR(get_mag_strength_tesla(-30, -110) * 1e9, 32597, 145 + 326); + EXPECT_NEAR(get_mag_strength_tesla(-30, -105) * 1e9, 31351, 145 + 314); + EXPECT_NEAR(get_mag_strength_tesla(-30, -100) * 1e9, 30036, 145 + 300); + EXPECT_NEAR(get_mag_strength_tesla(-30, -95) * 1e9, 28666, 145 + 287); + EXPECT_NEAR(get_mag_strength_tesla(-30, -90) * 1e9, 27279, 145 + 273); + EXPECT_NEAR(get_mag_strength_tesla(-30, -85) * 1e9, 25935, 145 + 259); + EXPECT_NEAR(get_mag_strength_tesla(-30, -80) * 1e9, 24708, 145 + 247); + EXPECT_NEAR(get_mag_strength_tesla(-30, -75) * 1e9, 23678, 145 + 237); + EXPECT_NEAR(get_mag_strength_tesla(-30, -70) * 1e9, 22907, 145 + 229); + EXPECT_NEAR(get_mag_strength_tesla(-30, -65) * 1e9, 22429, 145 + 224); + EXPECT_NEAR(get_mag_strength_tesla(-30, -60) * 1e9, 22237, 145 + 222); + EXPECT_NEAR(get_mag_strength_tesla(-30, -55) * 1e9, 22285, 145 + 223); + EXPECT_NEAR(get_mag_strength_tesla(-30, -50) * 1e9, 22503, 145 + 225); + EXPECT_NEAR(get_mag_strength_tesla(-30, -45) * 1e9, 22819, 145 + 228); + EXPECT_NEAR(get_mag_strength_tesla(-30, -40) * 1e9, 23172, 145 + 232); + EXPECT_NEAR(get_mag_strength_tesla(-30, -35) * 1e9, 23528, 145 + 235); + EXPECT_NEAR(get_mag_strength_tesla(-30, -30) * 1e9, 23872, 145 + 239); + EXPECT_NEAR(get_mag_strength_tesla(-30, -25) * 1e9, 24202, 145 + 242); + EXPECT_NEAR(get_mag_strength_tesla(-30, -20) * 1e9, 24508, 145 + 245); + EXPECT_NEAR(get_mag_strength_tesla(-30, -15) * 1e9, 24776, 145 + 248); + EXPECT_NEAR(get_mag_strength_tesla(-30, -10) * 1e9, 24980, 145 + 250); + EXPECT_NEAR(get_mag_strength_tesla(-30, -5) * 1e9, 25108, 145 + 251); + EXPECT_NEAR(get_mag_strength_tesla(-30, 0) * 1e9, 25167, 145 + 252); + EXPECT_NEAR(get_mag_strength_tesla(-30, 5) * 1e9, 25198, 145 + 252); + EXPECT_NEAR(get_mag_strength_tesla(-30, 10) * 1e9, 25280, 145 + 253); + EXPECT_NEAR(get_mag_strength_tesla(-30, 15) * 1e9, 25510, 145 + 255); + EXPECT_NEAR(get_mag_strength_tesla(-30, 20) * 1e9, 25997, 145 + 260); + EXPECT_NEAR(get_mag_strength_tesla(-30, 25) * 1e9, 26831, 145 + 268); + EXPECT_NEAR(get_mag_strength_tesla(-30, 30) * 1e9, 28071, 145 + 281); + EXPECT_NEAR(get_mag_strength_tesla(-30, 35) * 1e9, 29731, 145 + 297); + EXPECT_NEAR(get_mag_strength_tesla(-30, 40) * 1e9, 31780, 145 + 318); + EXPECT_NEAR(get_mag_strength_tesla(-30, 45) * 1e9, 34142, 145 + 341); + EXPECT_NEAR(get_mag_strength_tesla(-30, 50) * 1e9, 36716, 145 + 367); + EXPECT_NEAR(get_mag_strength_tesla(-30, 55) * 1e9, 39386, 145 + 394); + EXPECT_NEAR(get_mag_strength_tesla(-30, 60) * 1e9, 42045, 145 + 420); + EXPECT_NEAR(get_mag_strength_tesla(-30, 65) * 1e9, 44603, 145 + 446); + EXPECT_NEAR(get_mag_strength_tesla(-30, 70) * 1e9, 46997, 145 + 470); + EXPECT_NEAR(get_mag_strength_tesla(-30, 75) * 1e9, 49186, 145 + 492); + EXPECT_NEAR(get_mag_strength_tesla(-30, 80) * 1e9, 51141, 145 + 511); + EXPECT_NEAR(get_mag_strength_tesla(-30, 85) * 1e9, 52842, 145 + 528); + EXPECT_NEAR(get_mag_strength_tesla(-30, 90) * 1e9, 54270, 145 + 543); + EXPECT_NEAR(get_mag_strength_tesla(-30, 95) * 1e9, 55414, 145 + 554); + EXPECT_NEAR(get_mag_strength_tesla(-30, 100) * 1e9, 56277, 145 + 563); + EXPECT_NEAR(get_mag_strength_tesla(-30, 105) * 1e9, 56882, 145 + 569); + EXPECT_NEAR(get_mag_strength_tesla(-30, 110) * 1e9, 57266, 145 + 573); + EXPECT_NEAR(get_mag_strength_tesla(-30, 115) * 1e9, 57469, 145 + 575); + EXPECT_NEAR(get_mag_strength_tesla(-30, 120) * 1e9, 57524, 145 + 575); + EXPECT_NEAR(get_mag_strength_tesla(-30, 125) * 1e9, 57449, 145 + 574); + EXPECT_NEAR(get_mag_strength_tesla(-30, 130) * 1e9, 57246, 145 + 572); + EXPECT_NEAR(get_mag_strength_tesla(-30, 135) * 1e9, 56913, 145 + 569); + EXPECT_NEAR(get_mag_strength_tesla(-30, 140) * 1e9, 56445, 145 + 564); + EXPECT_NEAR(get_mag_strength_tesla(-30, 145) * 1e9, 55843, 145 + 558); + EXPECT_NEAR(get_mag_strength_tesla(-30, 150) * 1e9, 55113, 145 + 551); EXPECT_NEAR(get_mag_strength_tesla(-30, 155) * 1e9, 54264, 145 + 543); - EXPECT_NEAR(get_mag_strength_tesla(-30, 160) * 1e9, 53307, 145 + 533); - EXPECT_NEAR(get_mag_strength_tesla(-30, 165) * 1e9, 52257, 145 + 523); - EXPECT_NEAR(get_mag_strength_tesla(-30, 170) * 1e9, 51134, 145 + 511); - EXPECT_NEAR(get_mag_strength_tesla(-30, 175) * 1e9, 49960, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, 180) * 1e9, 48759, 145 + 488); - EXPECT_NEAR(get_mag_strength_tesla(-25, -180) * 1e9, 46005, 145 + 460); - EXPECT_NEAR(get_mag_strength_tesla(-25, -175) * 1e9, 44846, 145 + 448); - EXPECT_NEAR(get_mag_strength_tesla(-25, -170) * 1e9, 43691, 145 + 437); - EXPECT_NEAR(get_mag_strength_tesla(-25, -165) * 1e9, 42544, 145 + 425); - EXPECT_NEAR(get_mag_strength_tesla(-25, -160) * 1e9, 41405, 145 + 414); - EXPECT_NEAR(get_mag_strength_tesla(-25, -155) * 1e9, 40276, 145 + 403); - EXPECT_NEAR(get_mag_strength_tesla(-25, -150) * 1e9, 39162, 145 + 392); - EXPECT_NEAR(get_mag_strength_tesla(-25, -145) * 1e9, 38072, 145 + 381); - EXPECT_NEAR(get_mag_strength_tesla(-25, -140) * 1e9, 37008, 145 + 370); - EXPECT_NEAR(get_mag_strength_tesla(-25, -135) * 1e9, 35975, 145 + 360); - EXPECT_NEAR(get_mag_strength_tesla(-25, -130) * 1e9, 34968, 145 + 350); - EXPECT_NEAR(get_mag_strength_tesla(-25, -125) * 1e9, 33983, 145 + 340); - EXPECT_NEAR(get_mag_strength_tesla(-25, -120) * 1e9, 33010, 145 + 330); - EXPECT_NEAR(get_mag_strength_tesla(-25, -115) * 1e9, 32030, 145 + 320); - EXPECT_NEAR(get_mag_strength_tesla(-25, -110) * 1e9, 31023, 145 + 310); - EXPECT_NEAR(get_mag_strength_tesla(-25, -105) * 1e9, 29964, 145 + 300); - EXPECT_NEAR(get_mag_strength_tesla(-25, -100) * 1e9, 28840, 145 + 288); - EXPECT_NEAR(get_mag_strength_tesla(-25, -95) * 1e9, 27656, 145 + 277); - EXPECT_NEAR(get_mag_strength_tesla(-25, -90) * 1e9, 26445, 145 + 264); - EXPECT_NEAR(get_mag_strength_tesla(-25, -85) * 1e9, 25263, 145 + 253); - EXPECT_NEAR(get_mag_strength_tesla(-25, -80) * 1e9, 24184, 145 + 242); - EXPECT_NEAR(get_mag_strength_tesla(-25, -75) * 1e9, 23285, 145 + 233); - EXPECT_NEAR(get_mag_strength_tesla(-25, -70) * 1e9, 22628, 145 + 226); - EXPECT_NEAR(get_mag_strength_tesla(-25, -65) * 1e9, 22247, 145 + 222); - EXPECT_NEAR(get_mag_strength_tesla(-25, -60) * 1e9, 22137, 145 + 221); - EXPECT_NEAR(get_mag_strength_tesla(-25, -55) * 1e9, 22257, 145 + 223); - EXPECT_NEAR(get_mag_strength_tesla(-25, -50) * 1e9, 22542, 145 + 225); - EXPECT_NEAR(get_mag_strength_tesla(-25, -45) * 1e9, 22925, 145 + 229); - EXPECT_NEAR(get_mag_strength_tesla(-25, -40) * 1e9, 23355, 145 + 234); - EXPECT_NEAR(get_mag_strength_tesla(-25, -35) * 1e9, 23806, 145 + 238); - EXPECT_NEAR(get_mag_strength_tesla(-25, -30) * 1e9, 24274, 145 + 243); - EXPECT_NEAR(get_mag_strength_tesla(-25, -25) * 1e9, 24758, 145 + 248); - EXPECT_NEAR(get_mag_strength_tesla(-25, -20) * 1e9, 25252, 145 + 253); - EXPECT_NEAR(get_mag_strength_tesla(-25, -15) * 1e9, 25725, 145 + 257); - EXPECT_NEAR(get_mag_strength_tesla(-25, -10) * 1e9, 26138, 145 + 261); - EXPECT_NEAR(get_mag_strength_tesla(-25, -5) * 1e9, 26454, 145 + 265); - EXPECT_NEAR(get_mag_strength_tesla(-25, 0) * 1e9, 26656, 145 + 267); - EXPECT_NEAR(get_mag_strength_tesla(-25, 5) * 1e9, 26767, 145 + 268); - EXPECT_NEAR(get_mag_strength_tesla(-25, 10) * 1e9, 26846, 145 + 268); - EXPECT_NEAR(get_mag_strength_tesla(-25, 15) * 1e9, 26987, 145 + 270); - EXPECT_NEAR(get_mag_strength_tesla(-25, 20) * 1e9, 27298, 145 + 273); - EXPECT_NEAR(get_mag_strength_tesla(-25, 25) * 1e9, 27886, 145 + 279); - EXPECT_NEAR(get_mag_strength_tesla(-25, 30) * 1e9, 28838, 145 + 288); - EXPECT_NEAR(get_mag_strength_tesla(-25, 35) * 1e9, 30203, 145 + 302); - EXPECT_NEAR(get_mag_strength_tesla(-25, 40) * 1e9, 31977, 145 + 320); - EXPECT_NEAR(get_mag_strength_tesla(-25, 45) * 1e9, 34100, 145 + 341); - EXPECT_NEAR(get_mag_strength_tesla(-25, 50) * 1e9, 36472, 145 + 365); - EXPECT_NEAR(get_mag_strength_tesla(-25, 55) * 1e9, 38971, 145 + 390); - EXPECT_NEAR(get_mag_strength_tesla(-25, 60) * 1e9, 41476, 145 + 415); - EXPECT_NEAR(get_mag_strength_tesla(-25, 65) * 1e9, 43888, 145 + 439); - EXPECT_NEAR(get_mag_strength_tesla(-25, 70) * 1e9, 46133, 145 + 461); - EXPECT_NEAR(get_mag_strength_tesla(-25, 75) * 1e9, 48166, 145 + 482); - EXPECT_NEAR(get_mag_strength_tesla(-25, 80) * 1e9, 49950, 145 + 499); - EXPECT_NEAR(get_mag_strength_tesla(-25, 85) * 1e9, 51457, 145 + 515); - EXPECT_NEAR(get_mag_strength_tesla(-25, 90) * 1e9, 52665, 145 + 527); - EXPECT_NEAR(get_mag_strength_tesla(-25, 95) * 1e9, 53563, 145 + 536); - EXPECT_NEAR(get_mag_strength_tesla(-25, 100) * 1e9, 54168, 145 + 542); - EXPECT_NEAR(get_mag_strength_tesla(-25, 105) * 1e9, 54519, 145 + 545); - EXPECT_NEAR(get_mag_strength_tesla(-25, 110) * 1e9, 54679, 145 + 547); - EXPECT_NEAR(get_mag_strength_tesla(-25, 115) * 1e9, 54705, 145 + 547); - EXPECT_NEAR(get_mag_strength_tesla(-25, 120) * 1e9, 54638, 145 + 546); - EXPECT_NEAR(get_mag_strength_tesla(-25, 125) * 1e9, 54491, 145 + 545); - EXPECT_NEAR(get_mag_strength_tesla(-25, 130) * 1e9, 54254, 145 + 543); - EXPECT_NEAR(get_mag_strength_tesla(-25, 135) * 1e9, 53908, 145 + 539); - EXPECT_NEAR(get_mag_strength_tesla(-25, 140) * 1e9, 53440, 145 + 534); - EXPECT_NEAR(get_mag_strength_tesla(-25, 145) * 1e9, 52847, 145 + 528); - EXPECT_NEAR(get_mag_strength_tesla(-25, 150) * 1e9, 52134, 145 + 521); + EXPECT_NEAR(get_mag_strength_tesla(-30, 160) * 1e9, 53306, 145 + 533); + EXPECT_NEAR(get_mag_strength_tesla(-30, 165) * 1e9, 52256, 145 + 523); + EXPECT_NEAR(get_mag_strength_tesla(-30, 170) * 1e9, 51132, 145 + 511); + EXPECT_NEAR(get_mag_strength_tesla(-30, 175) * 1e9, 49958, 145 + 500); + EXPECT_NEAR(get_mag_strength_tesla(-30, 180) * 1e9, 48756, 145 + 488); + EXPECT_NEAR(get_mag_strength_tesla(-25, -180) * 1e9, 46003, 145 + 460); + EXPECT_NEAR(get_mag_strength_tesla(-25, -175) * 1e9, 44843, 145 + 448); + EXPECT_NEAR(get_mag_strength_tesla(-25, -170) * 1e9, 43688, 145 + 437); + EXPECT_NEAR(get_mag_strength_tesla(-25, -165) * 1e9, 42540, 145 + 425); + EXPECT_NEAR(get_mag_strength_tesla(-25, -160) * 1e9, 41400, 145 + 414); + EXPECT_NEAR(get_mag_strength_tesla(-25, -155) * 1e9, 40270, 145 + 403); + EXPECT_NEAR(get_mag_strength_tesla(-25, -150) * 1e9, 39156, 145 + 392); + EXPECT_NEAR(get_mag_strength_tesla(-25, -145) * 1e9, 38065, 145 + 381); + EXPECT_NEAR(get_mag_strength_tesla(-25, -140) * 1e9, 37002, 145 + 370); + EXPECT_NEAR(get_mag_strength_tesla(-25, -135) * 1e9, 35968, 145 + 360); + EXPECT_NEAR(get_mag_strength_tesla(-25, -130) * 1e9, 34961, 145 + 350); + EXPECT_NEAR(get_mag_strength_tesla(-25, -125) * 1e9, 33976, 145 + 340); + EXPECT_NEAR(get_mag_strength_tesla(-25, -120) * 1e9, 33002, 145 + 330); + EXPECT_NEAR(get_mag_strength_tesla(-25, -115) * 1e9, 32022, 145 + 320); + EXPECT_NEAR(get_mag_strength_tesla(-25, -110) * 1e9, 31015, 145 + 310); + EXPECT_NEAR(get_mag_strength_tesla(-25, -105) * 1e9, 29956, 145 + 300); + EXPECT_NEAR(get_mag_strength_tesla(-25, -100) * 1e9, 28831, 145 + 288); + EXPECT_NEAR(get_mag_strength_tesla(-25, -95) * 1e9, 27647, 145 + 276); + EXPECT_NEAR(get_mag_strength_tesla(-25, -90) * 1e9, 26436, 145 + 264); + EXPECT_NEAR(get_mag_strength_tesla(-25, -85) * 1e9, 25254, 145 + 253); + EXPECT_NEAR(get_mag_strength_tesla(-25, -80) * 1e9, 24176, 145 + 242); + EXPECT_NEAR(get_mag_strength_tesla(-25, -75) * 1e9, 23277, 145 + 233); + EXPECT_NEAR(get_mag_strength_tesla(-25, -70) * 1e9, 22621, 145 + 226); + EXPECT_NEAR(get_mag_strength_tesla(-25, -65) * 1e9, 22242, 145 + 222); + EXPECT_NEAR(get_mag_strength_tesla(-25, -60) * 1e9, 22133, 145 + 221); + EXPECT_NEAR(get_mag_strength_tesla(-25, -55) * 1e9, 22255, 145 + 223); + EXPECT_NEAR(get_mag_strength_tesla(-25, -50) * 1e9, 22541, 145 + 225); + EXPECT_NEAR(get_mag_strength_tesla(-25, -45) * 1e9, 22924, 145 + 229); + EXPECT_NEAR(get_mag_strength_tesla(-25, -40) * 1e9, 23354, 145 + 234); + EXPECT_NEAR(get_mag_strength_tesla(-25, -35) * 1e9, 23805, 145 + 238); + EXPECT_NEAR(get_mag_strength_tesla(-25, -30) * 1e9, 24272, 145 + 243); + EXPECT_NEAR(get_mag_strength_tesla(-25, -25) * 1e9, 24757, 145 + 248); + EXPECT_NEAR(get_mag_strength_tesla(-25, -20) * 1e9, 25249, 145 + 252); + EXPECT_NEAR(get_mag_strength_tesla(-25, -15) * 1e9, 25722, 145 + 257); + EXPECT_NEAR(get_mag_strength_tesla(-25, -10) * 1e9, 26133, 145 + 261); + EXPECT_NEAR(get_mag_strength_tesla(-25, -5) * 1e9, 26446, 145 + 264); + EXPECT_NEAR(get_mag_strength_tesla(-25, 0) * 1e9, 26647, 145 + 266); + EXPECT_NEAR(get_mag_strength_tesla(-25, 5) * 1e9, 26756, 145 + 268); + EXPECT_NEAR(get_mag_strength_tesla(-25, 10) * 1e9, 26835, 145 + 268); + EXPECT_NEAR(get_mag_strength_tesla(-25, 15) * 1e9, 26977, 145 + 270); + EXPECT_NEAR(get_mag_strength_tesla(-25, 20) * 1e9, 27290, 145 + 273); + EXPECT_NEAR(get_mag_strength_tesla(-25, 25) * 1e9, 27881, 145 + 279); + EXPECT_NEAR(get_mag_strength_tesla(-25, 30) * 1e9, 28837, 145 + 288); + EXPECT_NEAR(get_mag_strength_tesla(-25, 35) * 1e9, 30207, 145 + 302); + EXPECT_NEAR(get_mag_strength_tesla(-25, 40) * 1e9, 31984, 145 + 320); + EXPECT_NEAR(get_mag_strength_tesla(-25, 45) * 1e9, 34111, 145 + 341); + EXPECT_NEAR(get_mag_strength_tesla(-25, 50) * 1e9, 36486, 145 + 365); + EXPECT_NEAR(get_mag_strength_tesla(-25, 55) * 1e9, 38986, 145 + 390); + EXPECT_NEAR(get_mag_strength_tesla(-25, 60) * 1e9, 41492, 145 + 415); + EXPECT_NEAR(get_mag_strength_tesla(-25, 65) * 1e9, 43903, 145 + 439); + EXPECT_NEAR(get_mag_strength_tesla(-25, 70) * 1e9, 46147, 145 + 461); + EXPECT_NEAR(get_mag_strength_tesla(-25, 75) * 1e9, 48178, 145 + 482); + EXPECT_NEAR(get_mag_strength_tesla(-25, 80) * 1e9, 49960, 145 + 500); + EXPECT_NEAR(get_mag_strength_tesla(-25, 85) * 1e9, 51465, 145 + 515); + EXPECT_NEAR(get_mag_strength_tesla(-25, 90) * 1e9, 52671, 145 + 527); + EXPECT_NEAR(get_mag_strength_tesla(-25, 95) * 1e9, 53568, 145 + 536); + EXPECT_NEAR(get_mag_strength_tesla(-25, 100) * 1e9, 54171, 145 + 542); + EXPECT_NEAR(get_mag_strength_tesla(-25, 105) * 1e9, 54522, 145 + 545); + EXPECT_NEAR(get_mag_strength_tesla(-25, 110) * 1e9, 54681, 145 + 547); + EXPECT_NEAR(get_mag_strength_tesla(-25, 115) * 1e9, 54707, 145 + 547); + EXPECT_NEAR(get_mag_strength_tesla(-25, 120) * 1e9, 54640, 145 + 546); + EXPECT_NEAR(get_mag_strength_tesla(-25, 125) * 1e9, 54492, 145 + 545); + EXPECT_NEAR(get_mag_strength_tesla(-25, 130) * 1e9, 54255, 145 + 543); + EXPECT_NEAR(get_mag_strength_tesla(-25, 135) * 1e9, 53910, 145 + 539); + EXPECT_NEAR(get_mag_strength_tesla(-25, 140) * 1e9, 53442, 145 + 534); + EXPECT_NEAR(get_mag_strength_tesla(-25, 145) * 1e9, 52848, 145 + 528); + EXPECT_NEAR(get_mag_strength_tesla(-25, 150) * 1e9, 52135, 145 + 521); EXPECT_NEAR(get_mag_strength_tesla(-25, 155) * 1e9, 51310, 145 + 513); EXPECT_NEAR(get_mag_strength_tesla(-25, 160) * 1e9, 50386, 145 + 504); EXPECT_NEAR(get_mag_strength_tesla(-25, 165) * 1e9, 49373, 145 + 494); - EXPECT_NEAR(get_mag_strength_tesla(-25, 170) * 1e9, 48290, 145 + 483); - EXPECT_NEAR(get_mag_strength_tesla(-25, 175) * 1e9, 47160, 145 + 472); - EXPECT_NEAR(get_mag_strength_tesla(-25, 180) * 1e9, 46005, 145 + 460); - EXPECT_NEAR(get_mag_strength_tesla(-20, -180) * 1e9, 43197, 145 + 432); - EXPECT_NEAR(get_mag_strength_tesla(-20, -175) * 1e9, 42120, 145 + 421); - EXPECT_NEAR(get_mag_strength_tesla(-20, -170) * 1e9, 41053, 145 + 411); - EXPECT_NEAR(get_mag_strength_tesla(-20, -165) * 1e9, 39998, 145 + 400); - EXPECT_NEAR(get_mag_strength_tesla(-20, -160) * 1e9, 38952, 145 + 390); - EXPECT_NEAR(get_mag_strength_tesla(-20, -155) * 1e9, 37916, 145 + 379); - EXPECT_NEAR(get_mag_strength_tesla(-20, -150) * 1e9, 36896, 145 + 369); - EXPECT_NEAR(get_mag_strength_tesla(-20, -145) * 1e9, 35898, 145 + 359); - EXPECT_NEAR(get_mag_strength_tesla(-20, -140) * 1e9, 34930, 145 + 349); - EXPECT_NEAR(get_mag_strength_tesla(-20, -135) * 1e9, 33998, 145 + 340); - EXPECT_NEAR(get_mag_strength_tesla(-20, -130) * 1e9, 33102, 145 + 331); - EXPECT_NEAR(get_mag_strength_tesla(-20, -125) * 1e9, 32243, 145 + 322); - EXPECT_NEAR(get_mag_strength_tesla(-20, -120) * 1e9, 31414, 145 + 314); - EXPECT_NEAR(get_mag_strength_tesla(-20, -115) * 1e9, 30598, 145 + 306); - EXPECT_NEAR(get_mag_strength_tesla(-20, -110) * 1e9, 29772, 145 + 298); - EXPECT_NEAR(get_mag_strength_tesla(-20, -105) * 1e9, 28909, 145 + 289); - EXPECT_NEAR(get_mag_strength_tesla(-20, -100) * 1e9, 27988, 145 + 280); - EXPECT_NEAR(get_mag_strength_tesla(-20, -95) * 1e9, 27009, 145 + 270); - EXPECT_NEAR(get_mag_strength_tesla(-20, -90) * 1e9, 25997, 145 + 260); - EXPECT_NEAR(get_mag_strength_tesla(-20, -85) * 1e9, 25002, 145 + 250); - EXPECT_NEAR(get_mag_strength_tesla(-20, -80) * 1e9, 24089, 145 + 241); - EXPECT_NEAR(get_mag_strength_tesla(-20, -75) * 1e9, 23327, 145 + 233); - EXPECT_NEAR(get_mag_strength_tesla(-20, -70) * 1e9, 22773, 145 + 228); - EXPECT_NEAR(get_mag_strength_tesla(-20, -65) * 1e9, 22462, 145 + 225); - EXPECT_NEAR(get_mag_strength_tesla(-20, -60) * 1e9, 22396, 145 + 224); - EXPECT_NEAR(get_mag_strength_tesla(-20, -55) * 1e9, 22544, 145 + 225); + EXPECT_NEAR(get_mag_strength_tesla(-25, 170) * 1e9, 48289, 145 + 483); + EXPECT_NEAR(get_mag_strength_tesla(-25, 175) * 1e9, 47158, 145 + 472); + EXPECT_NEAR(get_mag_strength_tesla(-25, 180) * 1e9, 46003, 145 + 460); + EXPECT_NEAR(get_mag_strength_tesla(-20, -180) * 1e9, 43196, 145 + 432); + EXPECT_NEAR(get_mag_strength_tesla(-20, -175) * 1e9, 42117, 145 + 421); + EXPECT_NEAR(get_mag_strength_tesla(-20, -170) * 1e9, 41050, 145 + 410); + EXPECT_NEAR(get_mag_strength_tesla(-20, -165) * 1e9, 39994, 145 + 400); + EXPECT_NEAR(get_mag_strength_tesla(-20, -160) * 1e9, 38947, 145 + 389); + EXPECT_NEAR(get_mag_strength_tesla(-20, -155) * 1e9, 37911, 145 + 379); + EXPECT_NEAR(get_mag_strength_tesla(-20, -150) * 1e9, 36890, 145 + 369); + EXPECT_NEAR(get_mag_strength_tesla(-20, -145) * 1e9, 35892, 145 + 359); + EXPECT_NEAR(get_mag_strength_tesla(-20, -140) * 1e9, 34924, 145 + 349); + EXPECT_NEAR(get_mag_strength_tesla(-20, -135) * 1e9, 33992, 145 + 340); + EXPECT_NEAR(get_mag_strength_tesla(-20, -130) * 1e9, 33096, 145 + 331); + EXPECT_NEAR(get_mag_strength_tesla(-20, -125) * 1e9, 32237, 145 + 322); + EXPECT_NEAR(get_mag_strength_tesla(-20, -120) * 1e9, 31407, 145 + 314); + EXPECT_NEAR(get_mag_strength_tesla(-20, -115) * 1e9, 30591, 145 + 306); + EXPECT_NEAR(get_mag_strength_tesla(-20, -110) * 1e9, 29765, 145 + 298); + EXPECT_NEAR(get_mag_strength_tesla(-20, -105) * 1e9, 28901, 145 + 289); + EXPECT_NEAR(get_mag_strength_tesla(-20, -100) * 1e9, 27980, 145 + 280); + EXPECT_NEAR(get_mag_strength_tesla(-20, -95) * 1e9, 27000, 145 + 270); + EXPECT_NEAR(get_mag_strength_tesla(-20, -90) * 1e9, 25988, 145 + 260); + EXPECT_NEAR(get_mag_strength_tesla(-20, -85) * 1e9, 24993, 145 + 250); + EXPECT_NEAR(get_mag_strength_tesla(-20, -80) * 1e9, 24080, 145 + 241); + EXPECT_NEAR(get_mag_strength_tesla(-20, -75) * 1e9, 23319, 145 + 233); + EXPECT_NEAR(get_mag_strength_tesla(-20, -70) * 1e9, 22766, 145 + 228); + EXPECT_NEAR(get_mag_strength_tesla(-20, -65) * 1e9, 22457, 145 + 225); + EXPECT_NEAR(get_mag_strength_tesla(-20, -60) * 1e9, 22392, 145 + 224); + EXPECT_NEAR(get_mag_strength_tesla(-20, -55) * 1e9, 22542, 145 + 225); EXPECT_NEAR(get_mag_strength_tesla(-20, -50) * 1e9, 22856, 145 + 229); - EXPECT_NEAR(get_mag_strength_tesla(-20, -45) * 1e9, 23276, 145 + 233); - EXPECT_NEAR(get_mag_strength_tesla(-20, -40) * 1e9, 23762, 145 + 238); - EXPECT_NEAR(get_mag_strength_tesla(-20, -35) * 1e9, 24294, 145 + 243); - EXPECT_NEAR(get_mag_strength_tesla(-20, -30) * 1e9, 24869, 145 + 249); - EXPECT_NEAR(get_mag_strength_tesla(-20, -25) * 1e9, 25490, 145 + 255); + EXPECT_NEAR(get_mag_strength_tesla(-20, -45) * 1e9, 23277, 145 + 233); + EXPECT_NEAR(get_mag_strength_tesla(-20, -40) * 1e9, 23763, 145 + 238); + EXPECT_NEAR(get_mag_strength_tesla(-20, -35) * 1e9, 24295, 145 + 243); + EXPECT_NEAR(get_mag_strength_tesla(-20, -30) * 1e9, 24870, 145 + 249); + EXPECT_NEAR(get_mag_strength_tesla(-20, -25) * 1e9, 25491, 145 + 255); EXPECT_NEAR(get_mag_strength_tesla(-20, -20) * 1e9, 26144, 145 + 261); - EXPECT_NEAR(get_mag_strength_tesla(-20, -15) * 1e9, 26797, 145 + 268); - EXPECT_NEAR(get_mag_strength_tesla(-20, -10) * 1e9, 27396, 145 + 274); - EXPECT_NEAR(get_mag_strength_tesla(-20, -5) * 1e9, 27887, 145 + 279); - EXPECT_NEAR(get_mag_strength_tesla(-20, 0) * 1e9, 28238, 145 + 282); - EXPECT_NEAR(get_mag_strength_tesla(-20, 5) * 1e9, 28452, 145 + 285); - EXPECT_NEAR(get_mag_strength_tesla(-20, 10) * 1e9, 28568, 145 + 286); - EXPECT_NEAR(get_mag_strength_tesla(-20, 15) * 1e9, 28661, 145 + 287); - EXPECT_NEAR(get_mag_strength_tesla(-20, 20) * 1e9, 28833, 145 + 288); - EXPECT_NEAR(get_mag_strength_tesla(-20, 25) * 1e9, 29195, 145 + 292); - EXPECT_NEAR(get_mag_strength_tesla(-20, 30) * 1e9, 29854, 145 + 299); - EXPECT_NEAR(get_mag_strength_tesla(-20, 35) * 1e9, 30890, 145 + 309); - EXPECT_NEAR(get_mag_strength_tesla(-20, 40) * 1e9, 32331, 145 + 323); - EXPECT_NEAR(get_mag_strength_tesla(-20, 45) * 1e9, 34141, 145 + 341); - EXPECT_NEAR(get_mag_strength_tesla(-20, 50) * 1e9, 36231, 145 + 362); - EXPECT_NEAR(get_mag_strength_tesla(-20, 55) * 1e9, 38477, 145 + 385); - EXPECT_NEAR(get_mag_strength_tesla(-20, 60) * 1e9, 40755, 145 + 408); - EXPECT_NEAR(get_mag_strength_tesla(-20, 65) * 1e9, 42960, 145 + 430); - EXPECT_NEAR(get_mag_strength_tesla(-20, 70) * 1e9, 45012, 145 + 450); - EXPECT_NEAR(get_mag_strength_tesla(-20, 75) * 1e9, 46859, 145 + 469); - EXPECT_NEAR(get_mag_strength_tesla(-20, 80) * 1e9, 48459, 145 + 485); - EXPECT_NEAR(get_mag_strength_tesla(-20, 85) * 1e9, 49772, 145 + 498); - EXPECT_NEAR(get_mag_strength_tesla(-20, 90) * 1e9, 50767, 145 + 508); - EXPECT_NEAR(get_mag_strength_tesla(-20, 95) * 1e9, 51432, 145 + 514); - EXPECT_NEAR(get_mag_strength_tesla(-20, 100) * 1e9, 51793, 145 + 518); - EXPECT_NEAR(get_mag_strength_tesla(-20, 105) * 1e9, 51909, 145 + 519); - EXPECT_NEAR(get_mag_strength_tesla(-20, 110) * 1e9, 51862, 145 + 519); - EXPECT_NEAR(get_mag_strength_tesla(-20, 115) * 1e9, 51729, 145 + 517); - EXPECT_NEAR(get_mag_strength_tesla(-20, 120) * 1e9, 51556, 145 + 516); - EXPECT_NEAR(get_mag_strength_tesla(-20, 125) * 1e9, 51350, 145 + 514); - EXPECT_NEAR(get_mag_strength_tesla(-20, 130) * 1e9, 51087, 145 + 511); - EXPECT_NEAR(get_mag_strength_tesla(-20, 135) * 1e9, 50733, 145 + 507); - EXPECT_NEAR(get_mag_strength_tesla(-20, 140) * 1e9, 50267, 145 + 503); - EXPECT_NEAR(get_mag_strength_tesla(-20, 145) * 1e9, 49682, 145 + 497); - EXPECT_NEAR(get_mag_strength_tesla(-20, 150) * 1e9, 48988, 145 + 490); - EXPECT_NEAR(get_mag_strength_tesla(-20, 155) * 1e9, 48196, 145 + 482); + EXPECT_NEAR(get_mag_strength_tesla(-20, -15) * 1e9, 26796, 145 + 268); + EXPECT_NEAR(get_mag_strength_tesla(-20, -10) * 1e9, 27393, 145 + 274); + EXPECT_NEAR(get_mag_strength_tesla(-20, -5) * 1e9, 27882, 145 + 279); + EXPECT_NEAR(get_mag_strength_tesla(-20, 0) * 1e9, 28231, 145 + 282); + EXPECT_NEAR(get_mag_strength_tesla(-20, 5) * 1e9, 28442, 145 + 284); + EXPECT_NEAR(get_mag_strength_tesla(-20, 10) * 1e9, 28557, 145 + 286); + EXPECT_NEAR(get_mag_strength_tesla(-20, 15) * 1e9, 28651, 145 + 287); + EXPECT_NEAR(get_mag_strength_tesla(-20, 20) * 1e9, 28824, 145 + 288); + EXPECT_NEAR(get_mag_strength_tesla(-20, 25) * 1e9, 29189, 145 + 292); + EXPECT_NEAR(get_mag_strength_tesla(-20, 30) * 1e9, 29852, 145 + 299); + EXPECT_NEAR(get_mag_strength_tesla(-20, 35) * 1e9, 30892, 145 + 309); + EXPECT_NEAR(get_mag_strength_tesla(-20, 40) * 1e9, 32337, 145 + 323); + EXPECT_NEAR(get_mag_strength_tesla(-20, 45) * 1e9, 34151, 145 + 342); + EXPECT_NEAR(get_mag_strength_tesla(-20, 50) * 1e9, 36243, 145 + 362); + EXPECT_NEAR(get_mag_strength_tesla(-20, 55) * 1e9, 38491, 145 + 385); + EXPECT_NEAR(get_mag_strength_tesla(-20, 60) * 1e9, 40770, 145 + 408); + EXPECT_NEAR(get_mag_strength_tesla(-20, 65) * 1e9, 42974, 145 + 430); + EXPECT_NEAR(get_mag_strength_tesla(-20, 70) * 1e9, 45026, 145 + 450); + EXPECT_NEAR(get_mag_strength_tesla(-20, 75) * 1e9, 46871, 145 + 469); + EXPECT_NEAR(get_mag_strength_tesla(-20, 80) * 1e9, 48468, 145 + 485); + EXPECT_NEAR(get_mag_strength_tesla(-20, 85) * 1e9, 49779, 145 + 498); + EXPECT_NEAR(get_mag_strength_tesla(-20, 90) * 1e9, 50772, 145 + 508); + EXPECT_NEAR(get_mag_strength_tesla(-20, 95) * 1e9, 51436, 145 + 514); + EXPECT_NEAR(get_mag_strength_tesla(-20, 100) * 1e9, 51796, 145 + 518); + EXPECT_NEAR(get_mag_strength_tesla(-20, 105) * 1e9, 51911, 145 + 519); + EXPECT_NEAR(get_mag_strength_tesla(-20, 110) * 1e9, 51864, 145 + 519); + EXPECT_NEAR(get_mag_strength_tesla(-20, 115) * 1e9, 51730, 145 + 517); + EXPECT_NEAR(get_mag_strength_tesla(-20, 120) * 1e9, 51558, 145 + 516); + EXPECT_NEAR(get_mag_strength_tesla(-20, 125) * 1e9, 51352, 145 + 514); + EXPECT_NEAR(get_mag_strength_tesla(-20, 130) * 1e9, 51088, 145 + 511); + EXPECT_NEAR(get_mag_strength_tesla(-20, 135) * 1e9, 50735, 145 + 507); + EXPECT_NEAR(get_mag_strength_tesla(-20, 140) * 1e9, 50269, 145 + 503); + EXPECT_NEAR(get_mag_strength_tesla(-20, 145) * 1e9, 49684, 145 + 497); + EXPECT_NEAR(get_mag_strength_tesla(-20, 150) * 1e9, 48989, 145 + 490); + EXPECT_NEAR(get_mag_strength_tesla(-20, 155) * 1e9, 48197, 145 + 482); EXPECT_NEAR(get_mag_strength_tesla(-20, 160) * 1e9, 47316, 145 + 473); EXPECT_NEAR(get_mag_strength_tesla(-20, 165) * 1e9, 46358, 145 + 464); EXPECT_NEAR(get_mag_strength_tesla(-20, 170) * 1e9, 45338, 145 + 453); - EXPECT_NEAR(get_mag_strength_tesla(-20, 175) * 1e9, 44276, 145 + 443); - EXPECT_NEAR(get_mag_strength_tesla(-20, 180) * 1e9, 43197, 145 + 432); - EXPECT_NEAR(get_mag_strength_tesla(-15, -180) * 1e9, 40442, 145 + 404); - EXPECT_NEAR(get_mag_strength_tesla(-15, -175) * 1e9, 39477, 145 + 395); - EXPECT_NEAR(get_mag_strength_tesla(-15, -170) * 1e9, 38530, 145 + 385); - EXPECT_NEAR(get_mag_strength_tesla(-15, -165) * 1e9, 37598, 145 + 376); - EXPECT_NEAR(get_mag_strength_tesla(-15, -160) * 1e9, 36680, 145 + 367); - EXPECT_NEAR(get_mag_strength_tesla(-15, -155) * 1e9, 35774, 145 + 358); - EXPECT_NEAR(get_mag_strength_tesla(-15, -150) * 1e9, 34885, 145 + 349); - EXPECT_NEAR(get_mag_strength_tesla(-15, -145) * 1e9, 34020, 145 + 340); - EXPECT_NEAR(get_mag_strength_tesla(-15, -140) * 1e9, 33188, 145 + 332); - EXPECT_NEAR(get_mag_strength_tesla(-15, -135) * 1e9, 32393, 145 + 324); - EXPECT_NEAR(get_mag_strength_tesla(-15, -130) * 1e9, 31640, 145 + 316); - EXPECT_NEAR(get_mag_strength_tesla(-15, -125) * 1e9, 30930, 145 + 309); - EXPECT_NEAR(get_mag_strength_tesla(-15, -120) * 1e9, 30259, 145 + 303); - EXPECT_NEAR(get_mag_strength_tesla(-15, -115) * 1e9, 29613, 145 + 296); - EXPECT_NEAR(get_mag_strength_tesla(-15, -110) * 1e9, 28970, 145 + 290); - EXPECT_NEAR(get_mag_strength_tesla(-15, -105) * 1e9, 28303, 145 + 283); - EXPECT_NEAR(get_mag_strength_tesla(-15, -100) * 1e9, 27588, 145 + 276); - EXPECT_NEAR(get_mag_strength_tesla(-15, -95) * 1e9, 26822, 145 + 268); - EXPECT_NEAR(get_mag_strength_tesla(-15, -90) * 1e9, 26019, 145 + 260); - EXPECT_NEAR(get_mag_strength_tesla(-15, -85) * 1e9, 25218, 145 + 252); - EXPECT_NEAR(get_mag_strength_tesla(-15, -80) * 1e9, 24471, 145 + 245); - EXPECT_NEAR(get_mag_strength_tesla(-15, -75) * 1e9, 23833, 145 + 238); - EXPECT_NEAR(get_mag_strength_tesla(-15, -70) * 1e9, 23356, 145 + 234); - EXPECT_NEAR(get_mag_strength_tesla(-15, -65) * 1e9, 23075, 145 + 231); - EXPECT_NEAR(get_mag_strength_tesla(-15, -60) * 1e9, 23004, 145 + 230); - EXPECT_NEAR(get_mag_strength_tesla(-15, -55) * 1e9, 23131, 145 + 231); + EXPECT_NEAR(get_mag_strength_tesla(-20, 175) * 1e9, 44275, 145 + 443); + EXPECT_NEAR(get_mag_strength_tesla(-20, 180) * 1e9, 43196, 145 + 432); + EXPECT_NEAR(get_mag_strength_tesla(-15, -180) * 1e9, 40441, 145 + 404); + EXPECT_NEAR(get_mag_strength_tesla(-15, -175) * 1e9, 39475, 145 + 395); + EXPECT_NEAR(get_mag_strength_tesla(-15, -170) * 1e9, 38527, 145 + 385); + EXPECT_NEAR(get_mag_strength_tesla(-15, -165) * 1e9, 37595, 145 + 376); + EXPECT_NEAR(get_mag_strength_tesla(-15, -160) * 1e9, 36675, 145 + 367); + EXPECT_NEAR(get_mag_strength_tesla(-15, -155) * 1e9, 35769, 145 + 358); + EXPECT_NEAR(get_mag_strength_tesla(-15, -150) * 1e9, 34880, 145 + 349); + EXPECT_NEAR(get_mag_strength_tesla(-15, -145) * 1e9, 34015, 145 + 340); + EXPECT_NEAR(get_mag_strength_tesla(-15, -140) * 1e9, 33183, 145 + 332); + EXPECT_NEAR(get_mag_strength_tesla(-15, -135) * 1e9, 32388, 145 + 324); + EXPECT_NEAR(get_mag_strength_tesla(-15, -130) * 1e9, 31635, 145 + 316); + EXPECT_NEAR(get_mag_strength_tesla(-15, -125) * 1e9, 30924, 145 + 309); + EXPECT_NEAR(get_mag_strength_tesla(-15, -120) * 1e9, 30253, 145 + 303); + EXPECT_NEAR(get_mag_strength_tesla(-15, -115) * 1e9, 29607, 145 + 296); + EXPECT_NEAR(get_mag_strength_tesla(-15, -110) * 1e9, 28964, 145 + 290); + EXPECT_NEAR(get_mag_strength_tesla(-15, -105) * 1e9, 28295, 145 + 283); + EXPECT_NEAR(get_mag_strength_tesla(-15, -100) * 1e9, 27580, 145 + 276); + EXPECT_NEAR(get_mag_strength_tesla(-15, -95) * 1e9, 26812, 145 + 268); + EXPECT_NEAR(get_mag_strength_tesla(-15, -90) * 1e9, 26009, 145 + 260); + EXPECT_NEAR(get_mag_strength_tesla(-15, -85) * 1e9, 25208, 145 + 252); + EXPECT_NEAR(get_mag_strength_tesla(-15, -80) * 1e9, 24461, 145 + 245); + EXPECT_NEAR(get_mag_strength_tesla(-15, -75) * 1e9, 23824, 145 + 238); + EXPECT_NEAR(get_mag_strength_tesla(-15, -70) * 1e9, 23349, 145 + 233); + EXPECT_NEAR(get_mag_strength_tesla(-15, -65) * 1e9, 23070, 145 + 231); + EXPECT_NEAR(get_mag_strength_tesla(-15, -60) * 1e9, 23000, 145 + 230); + EXPECT_NEAR(get_mag_strength_tesla(-15, -55) * 1e9, 23130, 145 + 231); EXPECT_NEAR(get_mag_strength_tesla(-15, -50) * 1e9, 23427, 145 + 234); - EXPECT_NEAR(get_mag_strength_tesla(-15, -45) * 1e9, 23850, 145 + 239); - EXPECT_NEAR(get_mag_strength_tesla(-15, -40) * 1e9, 24368, 145 + 244); - EXPECT_NEAR(get_mag_strength_tesla(-15, -35) * 1e9, 24958, 145 + 250); - EXPECT_NEAR(get_mag_strength_tesla(-15, -30) * 1e9, 25613, 145 + 256); - EXPECT_NEAR(get_mag_strength_tesla(-15, -25) * 1e9, 26330, 145 + 263); - EXPECT_NEAR(get_mag_strength_tesla(-15, -20) * 1e9, 27090, 145 + 271); - EXPECT_NEAR(get_mag_strength_tesla(-15, -15) * 1e9, 27859, 145 + 279); + EXPECT_NEAR(get_mag_strength_tesla(-15, -45) * 1e9, 23852, 145 + 239); + EXPECT_NEAR(get_mag_strength_tesla(-15, -40) * 1e9, 24370, 145 + 244); + EXPECT_NEAR(get_mag_strength_tesla(-15, -35) * 1e9, 24960, 145 + 250); + EXPECT_NEAR(get_mag_strength_tesla(-15, -30) * 1e9, 25616, 145 + 256); + EXPECT_NEAR(get_mag_strength_tesla(-15, -25) * 1e9, 26332, 145 + 263); + EXPECT_NEAR(get_mag_strength_tesla(-15, -20) * 1e9, 27093, 145 + 271); + EXPECT_NEAR(get_mag_strength_tesla(-15, -15) * 1e9, 27860, 145 + 279); EXPECT_NEAR(get_mag_strength_tesla(-15, -10) * 1e9, 28579, 145 + 286); - EXPECT_NEAR(get_mag_strength_tesla(-15, -5) * 1e9, 29194, 145 + 292); - EXPECT_NEAR(get_mag_strength_tesla(-15, 0) * 1e9, 29662, 145 + 297); - EXPECT_NEAR(get_mag_strength_tesla(-15, 5) * 1e9, 29970, 145 + 300); - EXPECT_NEAR(get_mag_strength_tesla(-15, 10) * 1e9, 30137, 145 + 301); - EXPECT_NEAR(get_mag_strength_tesla(-15, 15) * 1e9, 30219, 145 + 302); - EXPECT_NEAR(get_mag_strength_tesla(-15, 20) * 1e9, 30299, 145 + 303); - EXPECT_NEAR(get_mag_strength_tesla(-15, 25) * 1e9, 30482, 145 + 305); - EXPECT_NEAR(get_mag_strength_tesla(-15, 30) * 1e9, 30886, 145 + 309); + EXPECT_NEAR(get_mag_strength_tesla(-15, -5) * 1e9, 29192, 145 + 292); + EXPECT_NEAR(get_mag_strength_tesla(-15, 0) * 1e9, 29657, 145 + 297); + EXPECT_NEAR(get_mag_strength_tesla(-15, 5) * 1e9, 29963, 145 + 300); + EXPECT_NEAR(get_mag_strength_tesla(-15, 10) * 1e9, 30129, 145 + 301); + EXPECT_NEAR(get_mag_strength_tesla(-15, 15) * 1e9, 30210, 145 + 302); + EXPECT_NEAR(get_mag_strength_tesla(-15, 20) * 1e9, 30290, 145 + 303); + EXPECT_NEAR(get_mag_strength_tesla(-15, 25) * 1e9, 30476, 145 + 305); + EXPECT_NEAR(get_mag_strength_tesla(-15, 30) * 1e9, 30883, 145 + 309); EXPECT_NEAR(get_mag_strength_tesla(-15, 35) * 1e9, 31612, 145 + 316); - EXPECT_NEAR(get_mag_strength_tesla(-15, 40) * 1e9, 32714, 145 + 327); - EXPECT_NEAR(get_mag_strength_tesla(-15, 45) * 1e9, 34182, 145 + 342); - EXPECT_NEAR(get_mag_strength_tesla(-15, 50) * 1e9, 35942, 145 + 359); - EXPECT_NEAR(get_mag_strength_tesla(-15, 55) * 1e9, 37879, 145 + 379); - EXPECT_NEAR(get_mag_strength_tesla(-15, 60) * 1e9, 39873, 145 + 399); - EXPECT_NEAR(get_mag_strength_tesla(-15, 65) * 1e9, 41819, 145 + 418); - EXPECT_NEAR(get_mag_strength_tesla(-15, 70) * 1e9, 43639, 145 + 436); - EXPECT_NEAR(get_mag_strength_tesla(-15, 75) * 1e9, 45278, 145 + 453); - EXPECT_NEAR(get_mag_strength_tesla(-15, 80) * 1e9, 46684, 145 + 467); - EXPECT_NEAR(get_mag_strength_tesla(-15, 85) * 1e9, 47810, 145 + 478); - EXPECT_NEAR(get_mag_strength_tesla(-15, 90) * 1e9, 48614, 145 + 486); - EXPECT_NEAR(get_mag_strength_tesla(-15, 95) * 1e9, 49081, 145 + 491); - EXPECT_NEAR(get_mag_strength_tesla(-15, 100) * 1e9, 49240, 145 + 492); - EXPECT_NEAR(get_mag_strength_tesla(-15, 105) * 1e9, 49163, 145 + 492); - EXPECT_NEAR(get_mag_strength_tesla(-15, 110) * 1e9, 48950, 145 + 490); + EXPECT_NEAR(get_mag_strength_tesla(-15, 40) * 1e9, 32718, 145 + 327); + EXPECT_NEAR(get_mag_strength_tesla(-15, 45) * 1e9, 34189, 145 + 342); + EXPECT_NEAR(get_mag_strength_tesla(-15, 50) * 1e9, 35952, 145 + 360); + EXPECT_NEAR(get_mag_strength_tesla(-15, 55) * 1e9, 37891, 145 + 379); + EXPECT_NEAR(get_mag_strength_tesla(-15, 60) * 1e9, 39886, 145 + 399); + EXPECT_NEAR(get_mag_strength_tesla(-15, 65) * 1e9, 41832, 145 + 418); + EXPECT_NEAR(get_mag_strength_tesla(-15, 70) * 1e9, 43652, 145 + 437); + EXPECT_NEAR(get_mag_strength_tesla(-15, 75) * 1e9, 45289, 145 + 453); + EXPECT_NEAR(get_mag_strength_tesla(-15, 80) * 1e9, 46694, 145 + 467); + EXPECT_NEAR(get_mag_strength_tesla(-15, 85) * 1e9, 47817, 145 + 478); + EXPECT_NEAR(get_mag_strength_tesla(-15, 90) * 1e9, 48619, 145 + 486); + EXPECT_NEAR(get_mag_strength_tesla(-15, 95) * 1e9, 49084, 145 + 491); + EXPECT_NEAR(get_mag_strength_tesla(-15, 100) * 1e9, 49242, 145 + 492); + EXPECT_NEAR(get_mag_strength_tesla(-15, 105) * 1e9, 49164, 145 + 492); + EXPECT_NEAR(get_mag_strength_tesla(-15, 110) * 1e9, 48951, 145 + 490); EXPECT_NEAR(get_mag_strength_tesla(-15, 115) * 1e9, 48690, 145 + 487); - EXPECT_NEAR(get_mag_strength_tesla(-15, 120) * 1e9, 48431, 145 + 484); - EXPECT_NEAR(get_mag_strength_tesla(-15, 125) * 1e9, 48175, 145 + 482); - EXPECT_NEAR(get_mag_strength_tesla(-15, 130) * 1e9, 47887, 145 + 479); - EXPECT_NEAR(get_mag_strength_tesla(-15, 135) * 1e9, 47522, 145 + 475); - EXPECT_NEAR(get_mag_strength_tesla(-15, 140) * 1e9, 47052, 145 + 471); - EXPECT_NEAR(get_mag_strength_tesla(-15, 145) * 1e9, 46472, 145 + 465); - EXPECT_NEAR(get_mag_strength_tesla(-15, 150) * 1e9, 45795, 145 + 458); - EXPECT_NEAR(get_mag_strength_tesla(-15, 155) * 1e9, 45037, 145 + 450); - EXPECT_NEAR(get_mag_strength_tesla(-15, 160) * 1e9, 44209, 145 + 442); + EXPECT_NEAR(get_mag_strength_tesla(-15, 120) * 1e9, 48432, 145 + 484); + EXPECT_NEAR(get_mag_strength_tesla(-15, 125) * 1e9, 48176, 145 + 482); + EXPECT_NEAR(get_mag_strength_tesla(-15, 130) * 1e9, 47888, 145 + 479); + EXPECT_NEAR(get_mag_strength_tesla(-15, 135) * 1e9, 47523, 145 + 475); + EXPECT_NEAR(get_mag_strength_tesla(-15, 140) * 1e9, 47053, 145 + 471); + EXPECT_NEAR(get_mag_strength_tesla(-15, 145) * 1e9, 46474, 145 + 465); + EXPECT_NEAR(get_mag_strength_tesla(-15, 150) * 1e9, 45797, 145 + 458); + EXPECT_NEAR(get_mag_strength_tesla(-15, 155) * 1e9, 45039, 145 + 450); + EXPECT_NEAR(get_mag_strength_tesla(-15, 160) * 1e9, 44210, 145 + 442); EXPECT_NEAR(get_mag_strength_tesla(-15, 165) * 1e9, 43321, 145 + 433); - EXPECT_NEAR(get_mag_strength_tesla(-15, 170) * 1e9, 42383, 145 + 424); + EXPECT_NEAR(get_mag_strength_tesla(-15, 170) * 1e9, 42384, 145 + 424); EXPECT_NEAR(get_mag_strength_tesla(-15, 175) * 1e9, 41416, 145 + 414); - EXPECT_NEAR(get_mag_strength_tesla(-15, 180) * 1e9, 40442, 145 + 404); - EXPECT_NEAR(get_mag_strength_tesla(-10, -180) * 1e9, 37895, 145 + 379); - EXPECT_NEAR(get_mag_strength_tesla(-10, -175) * 1e9, 37072, 145 + 371); - EXPECT_NEAR(get_mag_strength_tesla(-10, -170) * 1e9, 36271, 145 + 363); - EXPECT_NEAR(get_mag_strength_tesla(-10, -165) * 1e9, 35491, 145 + 355); - EXPECT_NEAR(get_mag_strength_tesla(-10, -160) * 1e9, 34728, 145 + 347); - EXPECT_NEAR(get_mag_strength_tesla(-10, -155) * 1e9, 33982, 145 + 340); - EXPECT_NEAR(get_mag_strength_tesla(-10, -150) * 1e9, 33257, 145 + 333); - EXPECT_NEAR(get_mag_strength_tesla(-10, -145) * 1e9, 32561, 145 + 326); - EXPECT_NEAR(get_mag_strength_tesla(-10, -140) * 1e9, 31900, 145 + 319); - EXPECT_NEAR(get_mag_strength_tesla(-10, -135) * 1e9, 31278, 145 + 313); - EXPECT_NEAR(get_mag_strength_tesla(-10, -130) * 1e9, 30696, 145 + 307); - EXPECT_NEAR(get_mag_strength_tesla(-10, -125) * 1e9, 30158, 145 + 302); - EXPECT_NEAR(get_mag_strength_tesla(-10, -120) * 1e9, 29658, 145 + 297); - EXPECT_NEAR(get_mag_strength_tesla(-10, -115) * 1e9, 29188, 145 + 292); - EXPECT_NEAR(get_mag_strength_tesla(-10, -110) * 1e9, 28727, 145 + 287); - EXPECT_NEAR(get_mag_strength_tesla(-10, -105) * 1e9, 28252, 145 + 283); - EXPECT_NEAR(get_mag_strength_tesla(-10, -100) * 1e9, 27741, 145 + 277); - EXPECT_NEAR(get_mag_strength_tesla(-10, -95) * 1e9, 27181, 145 + 272); - EXPECT_NEAR(get_mag_strength_tesla(-10, -90) * 1e9, 26581, 145 + 266); - EXPECT_NEAR(get_mag_strength_tesla(-10, -85) * 1e9, 25961, 145 + 260); - EXPECT_NEAR(get_mag_strength_tesla(-10, -80) * 1e9, 25359, 145 + 254); - EXPECT_NEAR(get_mag_strength_tesla(-10, -75) * 1e9, 24818, 145 + 248); - EXPECT_NEAR(get_mag_strength_tesla(-10, -70) * 1e9, 24381, 145 + 244); - EXPECT_NEAR(get_mag_strength_tesla(-10, -65) * 1e9, 24087, 145 + 241); - EXPECT_NEAR(get_mag_strength_tesla(-10, -60) * 1e9, 23960, 145 + 240); - EXPECT_NEAR(get_mag_strength_tesla(-10, -55) * 1e9, 24014, 145 + 240); + EXPECT_NEAR(get_mag_strength_tesla(-15, 180) * 1e9, 40441, 145 + 404); + EXPECT_NEAR(get_mag_strength_tesla(-10, -180) * 1e9, 37894, 145 + 379); + EXPECT_NEAR(get_mag_strength_tesla(-10, -175) * 1e9, 37070, 145 + 371); + EXPECT_NEAR(get_mag_strength_tesla(-10, -170) * 1e9, 36269, 145 + 363); + EXPECT_NEAR(get_mag_strength_tesla(-10, -165) * 1e9, 35488, 145 + 355); + EXPECT_NEAR(get_mag_strength_tesla(-10, -160) * 1e9, 34724, 145 + 347); + EXPECT_NEAR(get_mag_strength_tesla(-10, -155) * 1e9, 33977, 145 + 340); + EXPECT_NEAR(get_mag_strength_tesla(-10, -150) * 1e9, 33252, 145 + 333); + EXPECT_NEAR(get_mag_strength_tesla(-10, -145) * 1e9, 32556, 145 + 326); + EXPECT_NEAR(get_mag_strength_tesla(-10, -140) * 1e9, 31895, 145 + 319); + EXPECT_NEAR(get_mag_strength_tesla(-10, -135) * 1e9, 31273, 145 + 313); + EXPECT_NEAR(get_mag_strength_tesla(-10, -130) * 1e9, 30692, 145 + 307); + EXPECT_NEAR(get_mag_strength_tesla(-10, -125) * 1e9, 30152, 145 + 302); + EXPECT_NEAR(get_mag_strength_tesla(-10, -120) * 1e9, 29653, 145 + 297); + EXPECT_NEAR(get_mag_strength_tesla(-10, -115) * 1e9, 29182, 145 + 292); + EXPECT_NEAR(get_mag_strength_tesla(-10, -110) * 1e9, 28721, 145 + 287); + EXPECT_NEAR(get_mag_strength_tesla(-10, -105) * 1e9, 28245, 145 + 282); + EXPECT_NEAR(get_mag_strength_tesla(-10, -100) * 1e9, 27732, 145 + 277); + EXPECT_NEAR(get_mag_strength_tesla(-10, -95) * 1e9, 27172, 145 + 272); + EXPECT_NEAR(get_mag_strength_tesla(-10, -90) * 1e9, 26571, 145 + 266); + EXPECT_NEAR(get_mag_strength_tesla(-10, -85) * 1e9, 25951, 145 + 260); + EXPECT_NEAR(get_mag_strength_tesla(-10, -80) * 1e9, 25349, 145 + 253); + EXPECT_NEAR(get_mag_strength_tesla(-10, -75) * 1e9, 24809, 145 + 248); + EXPECT_NEAR(get_mag_strength_tesla(-10, -70) * 1e9, 24373, 145 + 244); + EXPECT_NEAR(get_mag_strength_tesla(-10, -65) * 1e9, 24080, 145 + 241); + EXPECT_NEAR(get_mag_strength_tesla(-10, -60) * 1e9, 23956, 145 + 240); + EXPECT_NEAR(get_mag_strength_tesla(-10, -55) * 1e9, 24012, 145 + 240); EXPECT_NEAR(get_mag_strength_tesla(-10, -50) * 1e9, 24242, 145 + 242); - EXPECT_NEAR(get_mag_strength_tesla(-10, -45) * 1e9, 24624, 145 + 246); - EXPECT_NEAR(get_mag_strength_tesla(-10, -40) * 1e9, 25133, 145 + 251); - EXPECT_NEAR(get_mag_strength_tesla(-10, -35) * 1e9, 25743, 145 + 257); - EXPECT_NEAR(get_mag_strength_tesla(-10, -30) * 1e9, 26433, 145 + 264); - EXPECT_NEAR(get_mag_strength_tesla(-10, -25) * 1e9, 27188, 145 + 272); - EXPECT_NEAR(get_mag_strength_tesla(-10, -20) * 1e9, 27985, 145 + 280); - EXPECT_NEAR(get_mag_strength_tesla(-10, -15) * 1e9, 28789, 145 + 288); - EXPECT_NEAR(get_mag_strength_tesla(-10, -10) * 1e9, 29550, 145 + 296); - EXPECT_NEAR(get_mag_strength_tesla(-10, -5) * 1e9, 30218, 145 + 302); - EXPECT_NEAR(get_mag_strength_tesla(-10, 0) * 1e9, 30749, 145 + 307); - EXPECT_NEAR(get_mag_strength_tesla(-10, 5) * 1e9, 31122, 145 + 311); - EXPECT_NEAR(get_mag_strength_tesla(-10, 10) * 1e9, 31342, 145 + 313); - EXPECT_NEAR(get_mag_strength_tesla(-10, 15) * 1e9, 31440, 145 + 314); - EXPECT_NEAR(get_mag_strength_tesla(-10, 20) * 1e9, 31478, 145 + 315); - EXPECT_NEAR(get_mag_strength_tesla(-10, 25) * 1e9, 31552, 145 + 316); - EXPECT_NEAR(get_mag_strength_tesla(-10, 30) * 1e9, 31776, 145 + 318); - EXPECT_NEAR(get_mag_strength_tesla(-10, 35) * 1e9, 32257, 145 + 323); - EXPECT_NEAR(get_mag_strength_tesla(-10, 40) * 1e9, 33068, 145 + 331); - EXPECT_NEAR(get_mag_strength_tesla(-10, 45) * 1e9, 34216, 145 + 342); - EXPECT_NEAR(get_mag_strength_tesla(-10, 50) * 1e9, 35644, 145 + 356); - EXPECT_NEAR(get_mag_strength_tesla(-10, 55) * 1e9, 37252, 145 + 373); - EXPECT_NEAR(get_mag_strength_tesla(-10, 60) * 1e9, 38932, 145 + 389); - EXPECT_NEAR(get_mag_strength_tesla(-10, 65) * 1e9, 40588, 145 + 406); - EXPECT_NEAR(get_mag_strength_tesla(-10, 70) * 1e9, 42150, 145 + 421); - EXPECT_NEAR(get_mag_strength_tesla(-10, 75) * 1e9, 43562, 145 + 436); - EXPECT_NEAR(get_mag_strength_tesla(-10, 80) * 1e9, 44770, 145 + 448); - EXPECT_NEAR(get_mag_strength_tesla(-10, 85) * 1e9, 45720, 145 + 457); - EXPECT_NEAR(get_mag_strength_tesla(-10, 90) * 1e9, 46361, 145 + 464); - EXPECT_NEAR(get_mag_strength_tesla(-10, 95) * 1e9, 46675, 145 + 467); - EXPECT_NEAR(get_mag_strength_tesla(-10, 100) * 1e9, 46689, 145 + 467); - EXPECT_NEAR(get_mag_strength_tesla(-10, 105) * 1e9, 46481, 145 + 465); + EXPECT_NEAR(get_mag_strength_tesla(-10, -45) * 1e9, 24626, 145 + 246); + EXPECT_NEAR(get_mag_strength_tesla(-10, -40) * 1e9, 25136, 145 + 251); + EXPECT_NEAR(get_mag_strength_tesla(-10, -35) * 1e9, 25746, 145 + 257); + EXPECT_NEAR(get_mag_strength_tesla(-10, -30) * 1e9, 26437, 145 + 264); + EXPECT_NEAR(get_mag_strength_tesla(-10, -25) * 1e9, 27192, 145 + 272); + EXPECT_NEAR(get_mag_strength_tesla(-10, -20) * 1e9, 27988, 145 + 280); + EXPECT_NEAR(get_mag_strength_tesla(-10, -15) * 1e9, 28791, 145 + 288); + EXPECT_NEAR(get_mag_strength_tesla(-10, -10) * 1e9, 29552, 145 + 296); + EXPECT_NEAR(get_mag_strength_tesla(-10, -5) * 1e9, 30217, 145 + 302); + EXPECT_NEAR(get_mag_strength_tesla(-10, 0) * 1e9, 30746, 145 + 307); + EXPECT_NEAR(get_mag_strength_tesla(-10, 5) * 1e9, 31118, 145 + 311); + EXPECT_NEAR(get_mag_strength_tesla(-10, 10) * 1e9, 31336, 145 + 313); + EXPECT_NEAR(get_mag_strength_tesla(-10, 15) * 1e9, 31432, 145 + 314); + EXPECT_NEAR(get_mag_strength_tesla(-10, 20) * 1e9, 31471, 145 + 315); + EXPECT_NEAR(get_mag_strength_tesla(-10, 25) * 1e9, 31546, 145 + 315); + EXPECT_NEAR(get_mag_strength_tesla(-10, 30) * 1e9, 31772, 145 + 318); + EXPECT_NEAR(get_mag_strength_tesla(-10, 35) * 1e9, 32256, 145 + 323); + EXPECT_NEAR(get_mag_strength_tesla(-10, 40) * 1e9, 33071, 145 + 331); + EXPECT_NEAR(get_mag_strength_tesla(-10, 45) * 1e9, 34222, 145 + 342); + EXPECT_NEAR(get_mag_strength_tesla(-10, 50) * 1e9, 35652, 145 + 357); + EXPECT_NEAR(get_mag_strength_tesla(-10, 55) * 1e9, 37262, 145 + 373); + EXPECT_NEAR(get_mag_strength_tesla(-10, 60) * 1e9, 38943, 145 + 389); + EXPECT_NEAR(get_mag_strength_tesla(-10, 65) * 1e9, 40600, 145 + 406); + EXPECT_NEAR(get_mag_strength_tesla(-10, 70) * 1e9, 42161, 145 + 422); + EXPECT_NEAR(get_mag_strength_tesla(-10, 75) * 1e9, 43572, 145 + 436); + EXPECT_NEAR(get_mag_strength_tesla(-10, 80) * 1e9, 44779, 145 + 448); + EXPECT_NEAR(get_mag_strength_tesla(-10, 85) * 1e9, 45726, 145 + 457); + EXPECT_NEAR(get_mag_strength_tesla(-10, 90) * 1e9, 46366, 145 + 464); + EXPECT_NEAR(get_mag_strength_tesla(-10, 95) * 1e9, 46678, 145 + 467); + EXPECT_NEAR(get_mag_strength_tesla(-10, 100) * 1e9, 46690, 145 + 467); + EXPECT_NEAR(get_mag_strength_tesla(-10, 105) * 1e9, 46482, 145 + 465); EXPECT_NEAR(get_mag_strength_tesla(-10, 110) * 1e9, 46156, 145 + 462); - EXPECT_NEAR(get_mag_strength_tesla(-10, 115) * 1e9, 45805, 145 + 458); - EXPECT_NEAR(get_mag_strength_tesla(-10, 120) * 1e9, 45479, 145 + 455); - EXPECT_NEAR(get_mag_strength_tesla(-10, 125) * 1e9, 45174, 145 + 452); - EXPECT_NEAR(get_mag_strength_tesla(-10, 130) * 1e9, 44850, 145 + 449); - EXPECT_NEAR(get_mag_strength_tesla(-10, 135) * 1e9, 44460, 145 + 445); - EXPECT_NEAR(get_mag_strength_tesla(-10, 140) * 1e9, 43974, 145 + 440); - EXPECT_NEAR(get_mag_strength_tesla(-10, 145) * 1e9, 43390, 145 + 434); - EXPECT_NEAR(get_mag_strength_tesla(-10, 150) * 1e9, 42727, 145 + 427); - EXPECT_NEAR(get_mag_strength_tesla(-10, 155) * 1e9, 42002, 145 + 420); - EXPECT_NEAR(get_mag_strength_tesla(-10, 160) * 1e9, 41231, 145 + 412); - EXPECT_NEAR(get_mag_strength_tesla(-10, 165) * 1e9, 40422, 145 + 404); - EXPECT_NEAR(get_mag_strength_tesla(-10, 170) * 1e9, 39585, 145 + 396); + EXPECT_NEAR(get_mag_strength_tesla(-10, 115) * 1e9, 45806, 145 + 458); + EXPECT_NEAR(get_mag_strength_tesla(-10, 120) * 1e9, 45480, 145 + 455); + EXPECT_NEAR(get_mag_strength_tesla(-10, 125) * 1e9, 45175, 145 + 452); + EXPECT_NEAR(get_mag_strength_tesla(-10, 130) * 1e9, 44852, 145 + 449); + EXPECT_NEAR(get_mag_strength_tesla(-10, 135) * 1e9, 44462, 145 + 445); + EXPECT_NEAR(get_mag_strength_tesla(-10, 140) * 1e9, 43976, 145 + 440); + EXPECT_NEAR(get_mag_strength_tesla(-10, 145) * 1e9, 43393, 145 + 434); + EXPECT_NEAR(get_mag_strength_tesla(-10, 150) * 1e9, 42729, 145 + 427); + EXPECT_NEAR(get_mag_strength_tesla(-10, 155) * 1e9, 42004, 145 + 420); + EXPECT_NEAR(get_mag_strength_tesla(-10, 160) * 1e9, 41232, 145 + 412); + EXPECT_NEAR(get_mag_strength_tesla(-10, 165) * 1e9, 40423, 145 + 404); + EXPECT_NEAR(get_mag_strength_tesla(-10, 170) * 1e9, 39586, 145 + 396); EXPECT_NEAR(get_mag_strength_tesla(-10, 175) * 1e9, 38737, 145 + 387); - EXPECT_NEAR(get_mag_strength_tesla(-10, 180) * 1e9, 37895, 145 + 379); - EXPECT_NEAR(get_mag_strength_tesla(-5, -180) * 1e9, 35734, 145 + 357); - EXPECT_NEAR(get_mag_strength_tesla(-5, -175) * 1e9, 35075, 145 + 351); - EXPECT_NEAR(get_mag_strength_tesla(-5, -170) * 1e9, 34442, 145 + 344); - EXPECT_NEAR(get_mag_strength_tesla(-5, -165) * 1e9, 33832, 145 + 338); - EXPECT_NEAR(get_mag_strength_tesla(-5, -160) * 1e9, 33241, 145 + 332); - EXPECT_NEAR(get_mag_strength_tesla(-5, -155) * 1e9, 32673, 145 + 327); - EXPECT_NEAR(get_mag_strength_tesla(-5, -150) * 1e9, 32133, 145 + 321); - EXPECT_NEAR(get_mag_strength_tesla(-5, -145) * 1e9, 31629, 145 + 316); - EXPECT_NEAR(get_mag_strength_tesla(-5, -140) * 1e9, 31165, 145 + 312); - EXPECT_NEAR(get_mag_strength_tesla(-5, -135) * 1e9, 30741, 145 + 307); - EXPECT_NEAR(get_mag_strength_tesla(-5, -130) * 1e9, 30356, 145 + 304); - EXPECT_NEAR(get_mag_strength_tesla(-5, -125) * 1e9, 30007, 145 + 300); - EXPECT_NEAR(get_mag_strength_tesla(-5, -120) * 1e9, 29691, 145 + 297); - EXPECT_NEAR(get_mag_strength_tesla(-5, -115) * 1e9, 29401, 145 + 294); - EXPECT_NEAR(get_mag_strength_tesla(-5, -110) * 1e9, 29122, 145 + 291); - EXPECT_NEAR(get_mag_strength_tesla(-5, -105) * 1e9, 28833, 145 + 288); - EXPECT_NEAR(get_mag_strength_tesla(-5, -100) * 1e9, 28512, 145 + 285); - EXPECT_NEAR(get_mag_strength_tesla(-5, -95) * 1e9, 28142, 145 + 281); - EXPECT_NEAR(get_mag_strength_tesla(-5, -90) * 1e9, 27718, 145 + 277); - EXPECT_NEAR(get_mag_strength_tesla(-5, -85) * 1e9, 27249, 145 + 272); - EXPECT_NEAR(get_mag_strength_tesla(-5, -80) * 1e9, 26756, 145 + 268); - EXPECT_NEAR(get_mag_strength_tesla(-5, -75) * 1e9, 26272, 145 + 263); - EXPECT_NEAR(get_mag_strength_tesla(-5, -70) * 1e9, 25836, 145 + 258); - EXPECT_NEAR(get_mag_strength_tesla(-5, -65) * 1e9, 25488, 145 + 255); - EXPECT_NEAR(get_mag_strength_tesla(-5, -60) * 1e9, 25265, 145 + 253); - EXPECT_NEAR(get_mag_strength_tesla(-5, -55) * 1e9, 25198, 145 + 252); - EXPECT_NEAR(get_mag_strength_tesla(-5, -50) * 1e9, 25306, 145 + 253); - EXPECT_NEAR(get_mag_strength_tesla(-5, -45) * 1e9, 25592, 145 + 256); - EXPECT_NEAR(get_mag_strength_tesla(-5, -40) * 1e9, 26036, 145 + 260); - EXPECT_NEAR(get_mag_strength_tesla(-5, -35) * 1e9, 26607, 145 + 266); - EXPECT_NEAR(get_mag_strength_tesla(-5, -30) * 1e9, 27272, 145 + 273); - EXPECT_NEAR(get_mag_strength_tesla(-5, -25) * 1e9, 28002, 145 + 280); - EXPECT_NEAR(get_mag_strength_tesla(-5, -20) * 1e9, 28767, 145 + 288); - EXPECT_NEAR(get_mag_strength_tesla(-5, -15) * 1e9, 29535, 145 + 295); - EXPECT_NEAR(get_mag_strength_tesla(-5, -10) * 1e9, 30266, 145 + 303); + EXPECT_NEAR(get_mag_strength_tesla(-10, 180) * 1e9, 37894, 145 + 379); + EXPECT_NEAR(get_mag_strength_tesla(-5, -180) * 1e9, 35733, 145 + 357); + EXPECT_NEAR(get_mag_strength_tesla(-5, -175) * 1e9, 35073, 145 + 351); + EXPECT_NEAR(get_mag_strength_tesla(-5, -170) * 1e9, 34440, 145 + 344); + EXPECT_NEAR(get_mag_strength_tesla(-5, -165) * 1e9, 33829, 145 + 338); + EXPECT_NEAR(get_mag_strength_tesla(-5, -160) * 1e9, 33238, 145 + 332); + EXPECT_NEAR(get_mag_strength_tesla(-5, -155) * 1e9, 32669, 145 + 327); + EXPECT_NEAR(get_mag_strength_tesla(-5, -150) * 1e9, 32129, 145 + 321); + EXPECT_NEAR(get_mag_strength_tesla(-5, -145) * 1e9, 31625, 145 + 316); + EXPECT_NEAR(get_mag_strength_tesla(-5, -140) * 1e9, 31161, 145 + 312); + EXPECT_NEAR(get_mag_strength_tesla(-5, -135) * 1e9, 30737, 145 + 307); + EXPECT_NEAR(get_mag_strength_tesla(-5, -130) * 1e9, 30352, 145 + 304); + EXPECT_NEAR(get_mag_strength_tesla(-5, -125) * 1e9, 30002, 145 + 300); + EXPECT_NEAR(get_mag_strength_tesla(-5, -120) * 1e9, 29686, 145 + 297); + EXPECT_NEAR(get_mag_strength_tesla(-5, -115) * 1e9, 29395, 145 + 294); + EXPECT_NEAR(get_mag_strength_tesla(-5, -110) * 1e9, 29115, 145 + 291); + EXPECT_NEAR(get_mag_strength_tesla(-5, -105) * 1e9, 28825, 145 + 288); + EXPECT_NEAR(get_mag_strength_tesla(-5, -100) * 1e9, 28503, 145 + 285); + EXPECT_NEAR(get_mag_strength_tesla(-5, -95) * 1e9, 28133, 145 + 281); + EXPECT_NEAR(get_mag_strength_tesla(-5, -90) * 1e9, 27708, 145 + 277); + EXPECT_NEAR(get_mag_strength_tesla(-5, -85) * 1e9, 27238, 145 + 272); + EXPECT_NEAR(get_mag_strength_tesla(-5, -80) * 1e9, 26745, 145 + 267); + EXPECT_NEAR(get_mag_strength_tesla(-5, -75) * 1e9, 26262, 145 + 263); + EXPECT_NEAR(get_mag_strength_tesla(-5, -70) * 1e9, 25827, 145 + 258); + EXPECT_NEAR(get_mag_strength_tesla(-5, -65) * 1e9, 25480, 145 + 255); + EXPECT_NEAR(get_mag_strength_tesla(-5, -60) * 1e9, 25259, 145 + 253); + EXPECT_NEAR(get_mag_strength_tesla(-5, -55) * 1e9, 25194, 145 + 252); + EXPECT_NEAR(get_mag_strength_tesla(-5, -50) * 1e9, 25305, 145 + 253); + EXPECT_NEAR(get_mag_strength_tesla(-5, -45) * 1e9, 25593, 145 + 256); + EXPECT_NEAR(get_mag_strength_tesla(-5, -40) * 1e9, 26038, 145 + 260); + EXPECT_NEAR(get_mag_strength_tesla(-5, -35) * 1e9, 26610, 145 + 266); + EXPECT_NEAR(get_mag_strength_tesla(-5, -30) * 1e9, 27277, 145 + 273); + EXPECT_NEAR(get_mag_strength_tesla(-5, -25) * 1e9, 28006, 145 + 280); + EXPECT_NEAR(get_mag_strength_tesla(-5, -20) * 1e9, 28771, 145 + 288); + EXPECT_NEAR(get_mag_strength_tesla(-5, -15) * 1e9, 29538, 145 + 295); + EXPECT_NEAR(get_mag_strength_tesla(-5, -10) * 1e9, 30268, 145 + 303); EXPECT_NEAR(get_mag_strength_tesla(-5, -5) * 1e9, 30922, 145 + 309); - EXPECT_NEAR(get_mag_strength_tesla(-5, 0) * 1e9, 31466, 145 + 315); - EXPECT_NEAR(get_mag_strength_tesla(-5, 5) * 1e9, 31874, 145 + 319); - EXPECT_NEAR(get_mag_strength_tesla(-5, 10) * 1e9, 32139, 145 + 321); - EXPECT_NEAR(get_mag_strength_tesla(-5, 15) * 1e9, 32275, 145 + 323); - EXPECT_NEAR(get_mag_strength_tesla(-5, 20) * 1e9, 32325, 145 + 323); - EXPECT_NEAR(get_mag_strength_tesla(-5, 25) * 1e9, 32367, 145 + 324); - EXPECT_NEAR(get_mag_strength_tesla(-5, 30) * 1e9, 32504, 145 + 325); - EXPECT_NEAR(get_mag_strength_tesla(-5, 35) * 1e9, 32839, 145 + 328); - EXPECT_NEAR(get_mag_strength_tesla(-5, 40) * 1e9, 33443, 145 + 334); - EXPECT_NEAR(get_mag_strength_tesla(-5, 45) * 1e9, 34333, 145 + 343); - EXPECT_NEAR(get_mag_strength_tesla(-5, 50) * 1e9, 35466, 145 + 355); - EXPECT_NEAR(get_mag_strength_tesla(-5, 55) * 1e9, 36761, 145 + 368); - EXPECT_NEAR(get_mag_strength_tesla(-5, 60) * 1e9, 38128, 145 + 381); - EXPECT_NEAR(get_mag_strength_tesla(-5, 65) * 1e9, 39491, 145 + 395); - EXPECT_NEAR(get_mag_strength_tesla(-5, 70) * 1e9, 40788, 145 + 408); - EXPECT_NEAR(get_mag_strength_tesla(-5, 75) * 1e9, 41968, 145 + 420); - EXPECT_NEAR(get_mag_strength_tesla(-5, 80) * 1e9, 42979, 145 + 430); - EXPECT_NEAR(get_mag_strength_tesla(-5, 85) * 1e9, 43763, 145 + 438); - EXPECT_NEAR(get_mag_strength_tesla(-5, 90) * 1e9, 44271, 145 + 443); - EXPECT_NEAR(get_mag_strength_tesla(-5, 95) * 1e9, 44478, 145 + 445); - EXPECT_NEAR(get_mag_strength_tesla(-5, 100) * 1e9, 44408, 145 + 444); - EXPECT_NEAR(get_mag_strength_tesla(-5, 105) * 1e9, 44135, 145 + 441); - EXPECT_NEAR(get_mag_strength_tesla(-5, 110) * 1e9, 43752, 145 + 438); + EXPECT_NEAR(get_mag_strength_tesla(-5, 0) * 1e9, 31464, 145 + 315); + EXPECT_NEAR(get_mag_strength_tesla(-5, 5) * 1e9, 31871, 145 + 319); + EXPECT_NEAR(get_mag_strength_tesla(-5, 10) * 1e9, 32135, 145 + 321); + EXPECT_NEAR(get_mag_strength_tesla(-5, 15) * 1e9, 32270, 145 + 323); + EXPECT_NEAR(get_mag_strength_tesla(-5, 20) * 1e9, 32320, 145 + 323); + EXPECT_NEAR(get_mag_strength_tesla(-5, 25) * 1e9, 32362, 145 + 324); + EXPECT_NEAR(get_mag_strength_tesla(-5, 30) * 1e9, 32500, 145 + 325); + EXPECT_NEAR(get_mag_strength_tesla(-5, 35) * 1e9, 32837, 145 + 328); + EXPECT_NEAR(get_mag_strength_tesla(-5, 40) * 1e9, 33445, 145 + 334); + EXPECT_NEAR(get_mag_strength_tesla(-5, 45) * 1e9, 34337, 145 + 343); + EXPECT_NEAR(get_mag_strength_tesla(-5, 50) * 1e9, 35472, 145 + 355); + EXPECT_NEAR(get_mag_strength_tesla(-5, 55) * 1e9, 36769, 145 + 368); + EXPECT_NEAR(get_mag_strength_tesla(-5, 60) * 1e9, 38138, 145 + 381); + EXPECT_NEAR(get_mag_strength_tesla(-5, 65) * 1e9, 39501, 145 + 395); + EXPECT_NEAR(get_mag_strength_tesla(-5, 70) * 1e9, 40797, 145 + 408); + EXPECT_NEAR(get_mag_strength_tesla(-5, 75) * 1e9, 41977, 145 + 420); + EXPECT_NEAR(get_mag_strength_tesla(-5, 80) * 1e9, 42987, 145 + 430); + EXPECT_NEAR(get_mag_strength_tesla(-5, 85) * 1e9, 43770, 145 + 438); + EXPECT_NEAR(get_mag_strength_tesla(-5, 90) * 1e9, 44275, 145 + 443); + EXPECT_NEAR(get_mag_strength_tesla(-5, 95) * 1e9, 44481, 145 + 445); + EXPECT_NEAR(get_mag_strength_tesla(-5, 100) * 1e9, 44410, 145 + 444); + EXPECT_NEAR(get_mag_strength_tesla(-5, 105) * 1e9, 44136, 145 + 441); + EXPECT_NEAR(get_mag_strength_tesla(-5, 110) * 1e9, 43753, 145 + 438); EXPECT_NEAR(get_mag_strength_tesla(-5, 115) * 1e9, 43346, 145 + 433); - EXPECT_NEAR(get_mag_strength_tesla(-5, 120) * 1e9, 42961, 145 + 430); - EXPECT_NEAR(get_mag_strength_tesla(-5, 125) * 1e9, 42596, 145 + 426); - EXPECT_NEAR(get_mag_strength_tesla(-5, 130) * 1e9, 42215, 145 + 422); - EXPECT_NEAR(get_mag_strength_tesla(-5, 135) * 1e9, 41774, 145 + 418); - EXPECT_NEAR(get_mag_strength_tesla(-5, 140) * 1e9, 41252, 145 + 413); - EXPECT_NEAR(get_mag_strength_tesla(-5, 145) * 1e9, 40651, 145 + 407); - EXPECT_NEAR(get_mag_strength_tesla(-5, 150) * 1e9, 39991, 145 + 400); - EXPECT_NEAR(get_mag_strength_tesla(-5, 155) * 1e9, 39295, 145 + 393); - EXPECT_NEAR(get_mag_strength_tesla(-5, 160) * 1e9, 38579, 145 + 386); - EXPECT_NEAR(get_mag_strength_tesla(-5, 165) * 1e9, 37855, 145 + 379); - EXPECT_NEAR(get_mag_strength_tesla(-5, 170) * 1e9, 37132, 145 + 371); - EXPECT_NEAR(get_mag_strength_tesla(-5, 175) * 1e9, 36421, 145 + 364); - EXPECT_NEAR(get_mag_strength_tesla(-5, 180) * 1e9, 35734, 145 + 357); + EXPECT_NEAR(get_mag_strength_tesla(-5, 120) * 1e9, 42962, 145 + 430); + EXPECT_NEAR(get_mag_strength_tesla(-5, 125) * 1e9, 42598, 145 + 426); + EXPECT_NEAR(get_mag_strength_tesla(-5, 130) * 1e9, 42216, 145 + 422); + EXPECT_NEAR(get_mag_strength_tesla(-5, 135) * 1e9, 41777, 145 + 418); + EXPECT_NEAR(get_mag_strength_tesla(-5, 140) * 1e9, 41255, 145 + 413); + EXPECT_NEAR(get_mag_strength_tesla(-5, 145) * 1e9, 40654, 145 + 407); + EXPECT_NEAR(get_mag_strength_tesla(-5, 150) * 1e9, 39994, 145 + 400); + EXPECT_NEAR(get_mag_strength_tesla(-5, 155) * 1e9, 39297, 145 + 393); + EXPECT_NEAR(get_mag_strength_tesla(-5, 160) * 1e9, 38581, 145 + 386); + EXPECT_NEAR(get_mag_strength_tesla(-5, 165) * 1e9, 37857, 145 + 379); + EXPECT_NEAR(get_mag_strength_tesla(-5, 170) * 1e9, 37133, 145 + 371); + EXPECT_NEAR(get_mag_strength_tesla(-5, 175) * 1e9, 36422, 145 + 364); + EXPECT_NEAR(get_mag_strength_tesla(-5, 180) * 1e9, 35733, 145 + 357); EXPECT_NEAR(get_mag_strength_tesla(0, -180) * 1e9, 34117, 145 + 341); - EXPECT_NEAR(get_mag_strength_tesla(0, -175) * 1e9, 33633, 145 + 336); - EXPECT_NEAR(get_mag_strength_tesla(0, -170) * 1e9, 33176, 145 + 332); - EXPECT_NEAR(get_mag_strength_tesla(0, -165) * 1e9, 32741, 145 + 327); - EXPECT_NEAR(get_mag_strength_tesla(0, -160) * 1e9, 32327, 145 + 323); - EXPECT_NEAR(get_mag_strength_tesla(0, -155) * 1e9, 31940, 145 + 319); - EXPECT_NEAR(get_mag_strength_tesla(0, -150) * 1e9, 31592, 145 + 316); - EXPECT_NEAR(get_mag_strength_tesla(0, -145) * 1e9, 31289, 145 + 313); - EXPECT_NEAR(get_mag_strength_tesla(0, -140) * 1e9, 31035, 145 + 310); - EXPECT_NEAR(get_mag_strength_tesla(0, -135) * 1e9, 30824, 145 + 308); - EXPECT_NEAR(get_mag_strength_tesla(0, -130) * 1e9, 30649, 145 + 306); - EXPECT_NEAR(get_mag_strength_tesla(0, -125) * 1e9, 30503, 145 + 305); - EXPECT_NEAR(get_mag_strength_tesla(0, -120) * 1e9, 30380, 145 + 304); - EXPECT_NEAR(get_mag_strength_tesla(0, -115) * 1e9, 30274, 145 + 303); - EXPECT_NEAR(get_mag_strength_tesla(0, -110) * 1e9, 30174, 145 + 302); - EXPECT_NEAR(get_mag_strength_tesla(0, -105) * 1e9, 30061, 145 + 301); - EXPECT_NEAR(get_mag_strength_tesla(0, -100) * 1e9, 29912, 145 + 299); - EXPECT_NEAR(get_mag_strength_tesla(0, -95) * 1e9, 29703, 145 + 297); - EXPECT_NEAR(get_mag_strength_tesla(0, -90) * 1e9, 29417, 145 + 294); - EXPECT_NEAR(get_mag_strength_tesla(0, -85) * 1e9, 29053, 145 + 291); - EXPECT_NEAR(get_mag_strength_tesla(0, -80) * 1e9, 28625, 145 + 286); - EXPECT_NEAR(get_mag_strength_tesla(0, -75) * 1e9, 28158, 145 + 282); - EXPECT_NEAR(get_mag_strength_tesla(0, -70) * 1e9, 27689, 145 + 277); - EXPECT_NEAR(get_mag_strength_tesla(0, -65) * 1e9, 27258, 145 + 273); - EXPECT_NEAR(get_mag_strength_tesla(0, -60) * 1e9, 26910, 145 + 269); - EXPECT_NEAR(get_mag_strength_tesla(0, -55) * 1e9, 26691, 145 + 267); - EXPECT_NEAR(get_mag_strength_tesla(0, -50) * 1e9, 26641, 145 + 266); - EXPECT_NEAR(get_mag_strength_tesla(0, -45) * 1e9, 26779, 145 + 268); - EXPECT_NEAR(get_mag_strength_tesla(0, -40) * 1e9, 27098, 145 + 271); - EXPECT_NEAR(get_mag_strength_tesla(0, -35) * 1e9, 27567, 145 + 276); - EXPECT_NEAR(get_mag_strength_tesla(0, -30) * 1e9, 28145, 145 + 281); - EXPECT_NEAR(get_mag_strength_tesla(0, -25) * 1e9, 28791, 145 + 288); - EXPECT_NEAR(get_mag_strength_tesla(0, -20) * 1e9, 29471, 145 + 295); - EXPECT_NEAR(get_mag_strength_tesla(0, -15) * 1e9, 30154, 145 + 302); - EXPECT_NEAR(get_mag_strength_tesla(0, -10) * 1e9, 30813, 145 + 308); - EXPECT_NEAR(get_mag_strength_tesla(0, -5) * 1e9, 31418, 145 + 314); - EXPECT_NEAR(get_mag_strength_tesla(0, 0) * 1e9, 31944, 145 + 319); - EXPECT_NEAR(get_mag_strength_tesla(0, 5) * 1e9, 32368, 145 + 324); - EXPECT_NEAR(get_mag_strength_tesla(0, 10) * 1e9, 32674, 145 + 327); - EXPECT_NEAR(get_mag_strength_tesla(0, 15) * 1e9, 32866, 145 + 329); - EXPECT_NEAR(get_mag_strength_tesla(0, 20) * 1e9, 32974, 145 + 330); - EXPECT_NEAR(get_mag_strength_tesla(0, 25) * 1e9, 33057, 145 + 331); - EXPECT_NEAR(get_mag_strength_tesla(0, 30) * 1e9, 33201, 145 + 332); + EXPECT_NEAR(get_mag_strength_tesla(0, -175) * 1e9, 33632, 145 + 336); + EXPECT_NEAR(get_mag_strength_tesla(0, -170) * 1e9, 33175, 145 + 332); + EXPECT_NEAR(get_mag_strength_tesla(0, -165) * 1e9, 32739, 145 + 327); + EXPECT_NEAR(get_mag_strength_tesla(0, -160) * 1e9, 32324, 145 + 323); + EXPECT_NEAR(get_mag_strength_tesla(0, -155) * 1e9, 31937, 145 + 319); + EXPECT_NEAR(get_mag_strength_tesla(0, -150) * 1e9, 31588, 145 + 316); + EXPECT_NEAR(get_mag_strength_tesla(0, -145) * 1e9, 31286, 145 + 313); + EXPECT_NEAR(get_mag_strength_tesla(0, -140) * 1e9, 31031, 145 + 310); + EXPECT_NEAR(get_mag_strength_tesla(0, -135) * 1e9, 30820, 145 + 308); + EXPECT_NEAR(get_mag_strength_tesla(0, -130) * 1e9, 30645, 145 + 306); + EXPECT_NEAR(get_mag_strength_tesla(0, -125) * 1e9, 30498, 145 + 305); + EXPECT_NEAR(get_mag_strength_tesla(0, -120) * 1e9, 30374, 145 + 304); + EXPECT_NEAR(get_mag_strength_tesla(0, -115) * 1e9, 30267, 145 + 303); + EXPECT_NEAR(get_mag_strength_tesla(0, -110) * 1e9, 30166, 145 + 302); + EXPECT_NEAR(get_mag_strength_tesla(0, -105) * 1e9, 30053, 145 + 301); + EXPECT_NEAR(get_mag_strength_tesla(0, -100) * 1e9, 29903, 145 + 299); + EXPECT_NEAR(get_mag_strength_tesla(0, -95) * 1e9, 29693, 145 + 297); + EXPECT_NEAR(get_mag_strength_tesla(0, -90) * 1e9, 29407, 145 + 294); + EXPECT_NEAR(get_mag_strength_tesla(0, -85) * 1e9, 29042, 145 + 290); + EXPECT_NEAR(get_mag_strength_tesla(0, -80) * 1e9, 28614, 145 + 286); + EXPECT_NEAR(get_mag_strength_tesla(0, -75) * 1e9, 28147, 145 + 281); + EXPECT_NEAR(get_mag_strength_tesla(0, -70) * 1e9, 27679, 145 + 277); + EXPECT_NEAR(get_mag_strength_tesla(0, -65) * 1e9, 27249, 145 + 272); + EXPECT_NEAR(get_mag_strength_tesla(0, -60) * 1e9, 26903, 145 + 269); + EXPECT_NEAR(get_mag_strength_tesla(0, -55) * 1e9, 26686, 145 + 267); + EXPECT_NEAR(get_mag_strength_tesla(0, -50) * 1e9, 26638, 145 + 266); + EXPECT_NEAR(get_mag_strength_tesla(0, -45) * 1e9, 26778, 145 + 268); + EXPECT_NEAR(get_mag_strength_tesla(0, -40) * 1e9, 27099, 145 + 271); + EXPECT_NEAR(get_mag_strength_tesla(0, -35) * 1e9, 27570, 145 + 276); + EXPECT_NEAR(get_mag_strength_tesla(0, -30) * 1e9, 28149, 145 + 281); + EXPECT_NEAR(get_mag_strength_tesla(0, -25) * 1e9, 28795, 145 + 288); + EXPECT_NEAR(get_mag_strength_tesla(0, -20) * 1e9, 29475, 145 + 295); + EXPECT_NEAR(get_mag_strength_tesla(0, -15) * 1e9, 30158, 145 + 302); + EXPECT_NEAR(get_mag_strength_tesla(0, -10) * 1e9, 30815, 145 + 308); + EXPECT_NEAR(get_mag_strength_tesla(0, -5) * 1e9, 31420, 145 + 314); + EXPECT_NEAR(get_mag_strength_tesla(0, 0) * 1e9, 31945, 145 + 319); + EXPECT_NEAR(get_mag_strength_tesla(0, 5) * 1e9, 32367, 145 + 324); + EXPECT_NEAR(get_mag_strength_tesla(0, 10) * 1e9, 32672, 145 + 327); + EXPECT_NEAR(get_mag_strength_tesla(0, 15) * 1e9, 32863, 145 + 329); + EXPECT_NEAR(get_mag_strength_tesla(0, 20) * 1e9, 32970, 145 + 330); + EXPECT_NEAR(get_mag_strength_tesla(0, 25) * 1e9, 33054, 145 + 331); + EXPECT_NEAR(get_mag_strength_tesla(0, 30) * 1e9, 33199, 145 + 332); EXPECT_NEAR(get_mag_strength_tesla(0, 35) * 1e9, 33490, 145 + 335); - EXPECT_NEAR(get_mag_strength_tesla(0, 40) * 1e9, 33985, 145 + 340); - EXPECT_NEAR(get_mag_strength_tesla(0, 45) * 1e9, 34697, 145 + 347); - EXPECT_NEAR(get_mag_strength_tesla(0, 50) * 1e9, 35597, 145 + 356); - EXPECT_NEAR(get_mag_strength_tesla(0, 55) * 1e9, 36624, 145 + 366); - EXPECT_NEAR(get_mag_strength_tesla(0, 60) * 1e9, 37712, 145 + 377); - EXPECT_NEAR(get_mag_strength_tesla(0, 65) * 1e9, 38805, 145 + 388); - EXPECT_NEAR(get_mag_strength_tesla(0, 70) * 1e9, 39854, 145 + 399); - EXPECT_NEAR(get_mag_strength_tesla(0, 75) * 1e9, 40816, 145 + 408); - EXPECT_NEAR(get_mag_strength_tesla(0, 80) * 1e9, 41641, 145 + 416); - EXPECT_NEAR(get_mag_strength_tesla(0, 85) * 1e9, 42277, 145 + 423); - EXPECT_NEAR(get_mag_strength_tesla(0, 90) * 1e9, 42676, 145 + 427); - EXPECT_NEAR(get_mag_strength_tesla(0, 95) * 1e9, 42816, 145 + 428); - EXPECT_NEAR(get_mag_strength_tesla(0, 100) * 1e9, 42715, 145 + 427); - EXPECT_NEAR(get_mag_strength_tesla(0, 105) * 1e9, 42427, 145 + 424); - EXPECT_NEAR(get_mag_strength_tesla(0, 110) * 1e9, 42030, 145 + 420); - EXPECT_NEAR(get_mag_strength_tesla(0, 115) * 1e9, 41590, 145 + 416); - EXPECT_NEAR(get_mag_strength_tesla(0, 120) * 1e9, 41146, 145 + 411); - EXPECT_NEAR(get_mag_strength_tesla(0, 125) * 1e9, 40699, 145 + 407); - EXPECT_NEAR(get_mag_strength_tesla(0, 130) * 1e9, 40227, 145 + 402); - EXPECT_NEAR(get_mag_strength_tesla(0, 135) * 1e9, 39703, 145 + 397); - EXPECT_NEAR(get_mag_strength_tesla(0, 140) * 1e9, 39117, 145 + 391); - EXPECT_NEAR(get_mag_strength_tesla(0, 145) * 1e9, 38475, 145 + 385); - EXPECT_NEAR(get_mag_strength_tesla(0, 150) * 1e9, 37802, 145 + 378); - EXPECT_NEAR(get_mag_strength_tesla(0, 155) * 1e9, 37123, 145 + 371); - EXPECT_NEAR(get_mag_strength_tesla(0, 160) * 1e9, 36456, 145 + 365); - EXPECT_NEAR(get_mag_strength_tesla(0, 165) * 1e9, 35813, 145 + 358); - EXPECT_NEAR(get_mag_strength_tesla(0, 170) * 1e9, 35206, 145 + 352); + EXPECT_NEAR(get_mag_strength_tesla(0, 40) * 1e9, 33986, 145 + 340); + EXPECT_NEAR(get_mag_strength_tesla(0, 45) * 1e9, 34701, 145 + 347); + EXPECT_NEAR(get_mag_strength_tesla(0, 50) * 1e9, 35602, 145 + 356); + EXPECT_NEAR(get_mag_strength_tesla(0, 55) * 1e9, 36631, 145 + 366); + EXPECT_NEAR(get_mag_strength_tesla(0, 60) * 1e9, 37720, 145 + 377); + EXPECT_NEAR(get_mag_strength_tesla(0, 65) * 1e9, 38814, 145 + 388); + EXPECT_NEAR(get_mag_strength_tesla(0, 70) * 1e9, 39863, 145 + 399); + EXPECT_NEAR(get_mag_strength_tesla(0, 75) * 1e9, 40824, 145 + 408); + EXPECT_NEAR(get_mag_strength_tesla(0, 80) * 1e9, 41649, 145 + 416); + EXPECT_NEAR(get_mag_strength_tesla(0, 85) * 1e9, 42283, 145 + 423); + EXPECT_NEAR(get_mag_strength_tesla(0, 90) * 1e9, 42681, 145 + 427); + EXPECT_NEAR(get_mag_strength_tesla(0, 95) * 1e9, 42820, 145 + 428); + EXPECT_NEAR(get_mag_strength_tesla(0, 100) * 1e9, 42718, 145 + 427); + EXPECT_NEAR(get_mag_strength_tesla(0, 105) * 1e9, 42429, 145 + 424); + EXPECT_NEAR(get_mag_strength_tesla(0, 110) * 1e9, 42031, 145 + 420); + EXPECT_NEAR(get_mag_strength_tesla(0, 115) * 1e9, 41591, 145 + 416); + EXPECT_NEAR(get_mag_strength_tesla(0, 120) * 1e9, 41147, 145 + 411); + EXPECT_NEAR(get_mag_strength_tesla(0, 125) * 1e9, 40700, 145 + 407); + EXPECT_NEAR(get_mag_strength_tesla(0, 130) * 1e9, 40229, 145 + 402); + EXPECT_NEAR(get_mag_strength_tesla(0, 135) * 1e9, 39706, 145 + 397); + EXPECT_NEAR(get_mag_strength_tesla(0, 140) * 1e9, 39119, 145 + 391); + EXPECT_NEAR(get_mag_strength_tesla(0, 145) * 1e9, 38478, 145 + 385); + EXPECT_NEAR(get_mag_strength_tesla(0, 150) * 1e9, 37805, 145 + 378); + EXPECT_NEAR(get_mag_strength_tesla(0, 155) * 1e9, 37125, 145 + 371); + EXPECT_NEAR(get_mag_strength_tesla(0, 160) * 1e9, 36458, 145 + 365); + EXPECT_NEAR(get_mag_strength_tesla(0, 165) * 1e9, 35815, 145 + 358); + EXPECT_NEAR(get_mag_strength_tesla(0, 170) * 1e9, 35207, 145 + 352); EXPECT_NEAR(get_mag_strength_tesla(0, 175) * 1e9, 34640, 145 + 346); EXPECT_NEAR(get_mag_strength_tesla(0, 180) * 1e9, 34117, 145 + 341); EXPECT_NEAR(get_mag_strength_tesla(5, -180) * 1e9, 33141, 145 + 331); - EXPECT_NEAR(get_mag_strength_tesla(5, -175) * 1e9, 32823, 145 + 328); - EXPECT_NEAR(get_mag_strength_tesla(5, -170) * 1e9, 32536, 145 + 325); - EXPECT_NEAR(get_mag_strength_tesla(5, -165) * 1e9, 32268, 145 + 323); - EXPECT_NEAR(get_mag_strength_tesla(5, -160) * 1e9, 32022, 145 + 320); - EXPECT_NEAR(get_mag_strength_tesla(5, -155) * 1e9, 31809, 145 + 318); - EXPECT_NEAR(get_mag_strength_tesla(5, -150) * 1e9, 31645, 145 + 316); - EXPECT_NEAR(get_mag_strength_tesla(5, -145) * 1e9, 31541, 145 + 315); - EXPECT_NEAR(get_mag_strength_tesla(5, -140) * 1e9, 31496, 145 + 315); - EXPECT_NEAR(get_mag_strength_tesla(5, -135) * 1e9, 31501, 145 + 315); - EXPECT_NEAR(get_mag_strength_tesla(5, -130) * 1e9, 31542, 145 + 315); - EXPECT_NEAR(get_mag_strength_tesla(5, -125) * 1e9, 31604, 145 + 316); - EXPECT_NEAR(get_mag_strength_tesla(5, -120) * 1e9, 31679, 145 + 317); - EXPECT_NEAR(get_mag_strength_tesla(5, -115) * 1e9, 31760, 145 + 318); - EXPECT_NEAR(get_mag_strength_tesla(5, -110) * 1e9, 31836, 145 + 318); - EXPECT_NEAR(get_mag_strength_tesla(5, -105) * 1e9, 31888, 145 + 319); - EXPECT_NEAR(get_mag_strength_tesla(5, -100) * 1e9, 31887, 145 + 319); - EXPECT_NEAR(get_mag_strength_tesla(5, -95) * 1e9, 31805, 145 + 318); - EXPECT_NEAR(get_mag_strength_tesla(5, -90) * 1e9, 31616, 145 + 316); - EXPECT_NEAR(get_mag_strength_tesla(5, -85) * 1e9, 31314, 145 + 313); - EXPECT_NEAR(get_mag_strength_tesla(5, -80) * 1e9, 30907, 145 + 309); - EXPECT_NEAR(get_mag_strength_tesla(5, -75) * 1e9, 30421, 145 + 304); - EXPECT_NEAR(get_mag_strength_tesla(5, -70) * 1e9, 29891, 145 + 299); - EXPECT_NEAR(get_mag_strength_tesla(5, -65) * 1e9, 29361, 145 + 294); - EXPECT_NEAR(get_mag_strength_tesla(5, -60) * 1e9, 28878, 145 + 289); - EXPECT_NEAR(get_mag_strength_tesla(5, -55) * 1e9, 28499, 145 + 285); - EXPECT_NEAR(get_mag_strength_tesla(5, -50) * 1e9, 28274, 145 + 283); - EXPECT_NEAR(get_mag_strength_tesla(5, -45) * 1e9, 28237, 145 + 282); + EXPECT_NEAR(get_mag_strength_tesla(5, -175) * 1e9, 32822, 145 + 328); + EXPECT_NEAR(get_mag_strength_tesla(5, -170) * 1e9, 32534, 145 + 325); + EXPECT_NEAR(get_mag_strength_tesla(5, -165) * 1e9, 32266, 145 + 323); + EXPECT_NEAR(get_mag_strength_tesla(5, -160) * 1e9, 32019, 145 + 320); + EXPECT_NEAR(get_mag_strength_tesla(5, -155) * 1e9, 31806, 145 + 318); + EXPECT_NEAR(get_mag_strength_tesla(5, -150) * 1e9, 31642, 145 + 316); + EXPECT_NEAR(get_mag_strength_tesla(5, -145) * 1e9, 31537, 145 + 315); + EXPECT_NEAR(get_mag_strength_tesla(5, -140) * 1e9, 31492, 145 + 315); + EXPECT_NEAR(get_mag_strength_tesla(5, -135) * 1e9, 31497, 145 + 315); + EXPECT_NEAR(get_mag_strength_tesla(5, -130) * 1e9, 31537, 145 + 315); + EXPECT_NEAR(get_mag_strength_tesla(5, -125) * 1e9, 31599, 145 + 316); + EXPECT_NEAR(get_mag_strength_tesla(5, -120) * 1e9, 31673, 145 + 317); + EXPECT_NEAR(get_mag_strength_tesla(5, -115) * 1e9, 31753, 145 + 318); + EXPECT_NEAR(get_mag_strength_tesla(5, -110) * 1e9, 31828, 145 + 318); + EXPECT_NEAR(get_mag_strength_tesla(5, -105) * 1e9, 31879, 145 + 319); + EXPECT_NEAR(get_mag_strength_tesla(5, -100) * 1e9, 31877, 145 + 319); + EXPECT_NEAR(get_mag_strength_tesla(5, -95) * 1e9, 31794, 145 + 318); + EXPECT_NEAR(get_mag_strength_tesla(5, -90) * 1e9, 31605, 145 + 316); + EXPECT_NEAR(get_mag_strength_tesla(5, -85) * 1e9, 31302, 145 + 313); + EXPECT_NEAR(get_mag_strength_tesla(5, -80) * 1e9, 30895, 145 + 309); + EXPECT_NEAR(get_mag_strength_tesla(5, -75) * 1e9, 30409, 145 + 304); + EXPECT_NEAR(get_mag_strength_tesla(5, -70) * 1e9, 29880, 145 + 299); + EXPECT_NEAR(get_mag_strength_tesla(5, -65) * 1e9, 29350, 145 + 294); + EXPECT_NEAR(get_mag_strength_tesla(5, -60) * 1e9, 28870, 145 + 289); + EXPECT_NEAR(get_mag_strength_tesla(5, -55) * 1e9, 28492, 145 + 285); + EXPECT_NEAR(get_mag_strength_tesla(5, -50) * 1e9, 28270, 145 + 283); + EXPECT_NEAR(get_mag_strength_tesla(5, -45) * 1e9, 28235, 145 + 282); EXPECT_NEAR(get_mag_strength_tesla(5, -40) * 1e9, 28391, 145 + 284); - EXPECT_NEAR(get_mag_strength_tesla(5, -35) * 1e9, 28710, 145 + 287); - EXPECT_NEAR(get_mag_strength_tesla(5, -30) * 1e9, 29152, 145 + 292); - EXPECT_NEAR(get_mag_strength_tesla(5, -25) * 1e9, 29672, 145 + 297); - EXPECT_NEAR(get_mag_strength_tesla(5, -20) * 1e9, 30237, 145 + 302); - EXPECT_NEAR(get_mag_strength_tesla(5, -15) * 1e9, 30817, 145 + 308); - EXPECT_NEAR(get_mag_strength_tesla(5, -10) * 1e9, 31392, 145 + 314); - EXPECT_NEAR(get_mag_strength_tesla(5, -5) * 1e9, 31942, 145 + 319); - EXPECT_NEAR(get_mag_strength_tesla(5, 0) * 1e9, 32444, 145 + 324); + EXPECT_NEAR(get_mag_strength_tesla(5, -35) * 1e9, 28711, 145 + 287); + EXPECT_NEAR(get_mag_strength_tesla(5, -30) * 1e9, 29154, 145 + 292); + EXPECT_NEAR(get_mag_strength_tesla(5, -25) * 1e9, 29676, 145 + 297); + EXPECT_NEAR(get_mag_strength_tesla(5, -20) * 1e9, 30240, 145 + 302); + EXPECT_NEAR(get_mag_strength_tesla(5, -15) * 1e9, 30820, 145 + 308); + EXPECT_NEAR(get_mag_strength_tesla(5, -10) * 1e9, 31395, 145 + 314); + EXPECT_NEAR(get_mag_strength_tesla(5, -5) * 1e9, 31944, 145 + 319); + EXPECT_NEAR(get_mag_strength_tesla(5, 0) * 1e9, 32446, 145 + 324); EXPECT_NEAR(get_mag_strength_tesla(5, 5) * 1e9, 32878, 145 + 329); EXPECT_NEAR(get_mag_strength_tesla(5, 10) * 1e9, 33226, 145 + 332); - EXPECT_NEAR(get_mag_strength_tesla(5, 15) * 1e9, 33488, 145 + 335); - EXPECT_NEAR(get_mag_strength_tesla(5, 20) * 1e9, 33685, 145 + 337); - EXPECT_NEAR(get_mag_strength_tesla(5, 25) * 1e9, 33863, 145 + 339); - EXPECT_NEAR(get_mag_strength_tesla(5, 30) * 1e9, 34085, 145 + 341); - EXPECT_NEAR(get_mag_strength_tesla(5, 35) * 1e9, 34411, 145 + 344); - EXPECT_NEAR(get_mag_strength_tesla(5, 40) * 1e9, 34878, 145 + 349); - EXPECT_NEAR(get_mag_strength_tesla(5, 45) * 1e9, 35491, 145 + 355); - EXPECT_NEAR(get_mag_strength_tesla(5, 50) * 1e9, 36226, 145 + 362); - EXPECT_NEAR(get_mag_strength_tesla(5, 55) * 1e9, 37045, 145 + 370); - EXPECT_NEAR(get_mag_strength_tesla(5, 60) * 1e9, 37908, 145 + 379); - EXPECT_NEAR(get_mag_strength_tesla(5, 65) * 1e9, 38778, 145 + 388); - EXPECT_NEAR(get_mag_strength_tesla(5, 70) * 1e9, 39620, 145 + 396); - EXPECT_NEAR(get_mag_strength_tesla(5, 75) * 1e9, 40397, 145 + 404); - EXPECT_NEAR(get_mag_strength_tesla(5, 80) * 1e9, 41064, 145 + 411); - EXPECT_NEAR(get_mag_strength_tesla(5, 85) * 1e9, 41576, 145 + 416); - EXPECT_NEAR(get_mag_strength_tesla(5, 90) * 1e9, 41893, 145 + 419); - EXPECT_NEAR(get_mag_strength_tesla(5, 95) * 1e9, 41996, 145 + 420); - EXPECT_NEAR(get_mag_strength_tesla(5, 100) * 1e9, 41895, 145 + 419); - EXPECT_NEAR(get_mag_strength_tesla(5, 105) * 1e9, 41624, 145 + 416); - EXPECT_NEAR(get_mag_strength_tesla(5, 110) * 1e9, 41232, 145 + 412); - EXPECT_NEAR(get_mag_strength_tesla(5, 115) * 1e9, 40765, 145 + 408); - EXPECT_NEAR(get_mag_strength_tesla(5, 120) * 1e9, 40249, 145 + 402); - EXPECT_NEAR(get_mag_strength_tesla(5, 125) * 1e9, 39694, 145 + 397); - EXPECT_NEAR(get_mag_strength_tesla(5, 130) * 1e9, 39096, 145 + 391); - EXPECT_NEAR(get_mag_strength_tesla(5, 135) * 1e9, 38452, 145 + 385); - EXPECT_NEAR(get_mag_strength_tesla(5, 140) * 1e9, 37765, 145 + 378); - EXPECT_NEAR(get_mag_strength_tesla(5, 145) * 1e9, 37054, 145 + 371); - EXPECT_NEAR(get_mag_strength_tesla(5, 150) * 1e9, 36343, 145 + 363); - EXPECT_NEAR(get_mag_strength_tesla(5, 155) * 1e9, 35658, 145 + 357); - EXPECT_NEAR(get_mag_strength_tesla(5, 160) * 1e9, 35020, 145 + 350); - EXPECT_NEAR(get_mag_strength_tesla(5, 165) * 1e9, 34444, 145 + 344); + EXPECT_NEAR(get_mag_strength_tesla(5, 15) * 1e9, 33487, 145 + 335); + EXPECT_NEAR(get_mag_strength_tesla(5, 20) * 1e9, 33684, 145 + 337); + EXPECT_NEAR(get_mag_strength_tesla(5, 25) * 1e9, 33862, 145 + 339); + EXPECT_NEAR(get_mag_strength_tesla(5, 30) * 1e9, 34084, 145 + 341); + EXPECT_NEAR(get_mag_strength_tesla(5, 35) * 1e9, 34412, 145 + 344); + EXPECT_NEAR(get_mag_strength_tesla(5, 40) * 1e9, 34881, 145 + 349); + EXPECT_NEAR(get_mag_strength_tesla(5, 45) * 1e9, 35495, 145 + 355); + EXPECT_NEAR(get_mag_strength_tesla(5, 50) * 1e9, 36232, 145 + 362); + EXPECT_NEAR(get_mag_strength_tesla(5, 55) * 1e9, 37052, 145 + 371); + EXPECT_NEAR(get_mag_strength_tesla(5, 60) * 1e9, 37916, 145 + 379); + EXPECT_NEAR(get_mag_strength_tesla(5, 65) * 1e9, 38787, 145 + 388); + EXPECT_NEAR(get_mag_strength_tesla(5, 70) * 1e9, 39629, 145 + 396); + EXPECT_NEAR(get_mag_strength_tesla(5, 75) * 1e9, 40405, 145 + 404); + EXPECT_NEAR(get_mag_strength_tesla(5, 80) * 1e9, 41072, 145 + 411); + EXPECT_NEAR(get_mag_strength_tesla(5, 85) * 1e9, 41583, 145 + 416); + EXPECT_NEAR(get_mag_strength_tesla(5, 90) * 1e9, 41899, 145 + 419); + EXPECT_NEAR(get_mag_strength_tesla(5, 95) * 1e9, 42002, 145 + 420); + EXPECT_NEAR(get_mag_strength_tesla(5, 100) * 1e9, 41899, 145 + 419); + EXPECT_NEAR(get_mag_strength_tesla(5, 105) * 1e9, 41627, 145 + 416); + EXPECT_NEAR(get_mag_strength_tesla(5, 110) * 1e9, 41235, 145 + 412); + EXPECT_NEAR(get_mag_strength_tesla(5, 115) * 1e9, 40767, 145 + 408); + EXPECT_NEAR(get_mag_strength_tesla(5, 120) * 1e9, 40251, 145 + 403); + EXPECT_NEAR(get_mag_strength_tesla(5, 125) * 1e9, 39696, 145 + 397); + EXPECT_NEAR(get_mag_strength_tesla(5, 130) * 1e9, 39099, 145 + 391); + EXPECT_NEAR(get_mag_strength_tesla(5, 135) * 1e9, 38454, 145 + 385); + EXPECT_NEAR(get_mag_strength_tesla(5, 140) * 1e9, 37768, 145 + 378); + EXPECT_NEAR(get_mag_strength_tesla(5, 145) * 1e9, 37057, 145 + 371); + EXPECT_NEAR(get_mag_strength_tesla(5, 150) * 1e9, 36346, 145 + 363); + EXPECT_NEAR(get_mag_strength_tesla(5, 155) * 1e9, 35661, 145 + 357); + EXPECT_NEAR(get_mag_strength_tesla(5, 160) * 1e9, 35022, 145 + 350); + EXPECT_NEAR(get_mag_strength_tesla(5, 165) * 1e9, 34446, 145 + 344); EXPECT_NEAR(get_mag_strength_tesla(5, 170) * 1e9, 33940, 145 + 339); EXPECT_NEAR(get_mag_strength_tesla(5, 175) * 1e9, 33508, 145 + 335); EXPECT_NEAR(get_mag_strength_tesla(5, 180) * 1e9, 33141, 145 + 331); EXPECT_NEAR(get_mag_strength_tesla(10, -180) * 1e9, 32823, 145 + 328); - EXPECT_NEAR(get_mag_strength_tesla(10, -175) * 1e9, 32645, 145 + 326); - EXPECT_NEAR(get_mag_strength_tesla(10, -170) * 1e9, 32503, 145 + 325); - EXPECT_NEAR(get_mag_strength_tesla(10, -165) * 1e9, 32384, 145 + 324); - EXPECT_NEAR(get_mag_strength_tesla(10, -160) * 1e9, 32289, 145 + 323); - EXPECT_NEAR(get_mag_strength_tesla(10, -155) * 1e9, 32236, 145 + 322); - EXPECT_NEAR(get_mag_strength_tesla(10, -150) * 1e9, 32244, 145 + 322); - EXPECT_NEAR(get_mag_strength_tesla(10, -145) * 1e9, 32326, 145 + 323); - EXPECT_NEAR(get_mag_strength_tesla(10, -140) * 1e9, 32481, 145 + 325); - EXPECT_NEAR(get_mag_strength_tesla(10, -135) * 1e9, 32694, 145 + 327); - EXPECT_NEAR(get_mag_strength_tesla(10, -130) * 1e9, 32946, 145 + 329); - EXPECT_NEAR(get_mag_strength_tesla(10, -125) * 1e9, 33215, 145 + 332); - EXPECT_NEAR(get_mag_strength_tesla(10, -120) * 1e9, 33489, 145 + 335); - EXPECT_NEAR(get_mag_strength_tesla(10, -115) * 1e9, 33756, 145 + 338); - EXPECT_NEAR(get_mag_strength_tesla(10, -110) * 1e9, 34002, 145 + 340); - EXPECT_NEAR(get_mag_strength_tesla(10, -105) * 1e9, 34205, 145 + 342); - EXPECT_NEAR(get_mag_strength_tesla(10, -100) * 1e9, 34332, 145 + 343); - EXPECT_NEAR(get_mag_strength_tesla(10, -95) * 1e9, 34348, 145 + 343); - EXPECT_NEAR(get_mag_strength_tesla(10, -90) * 1e9, 34223, 145 + 342); - EXPECT_NEAR(get_mag_strength_tesla(10, -85) * 1e9, 33947, 145 + 339); - EXPECT_NEAR(get_mag_strength_tesla(10, -80) * 1e9, 33529, 145 + 335); - EXPECT_NEAR(get_mag_strength_tesla(10, -75) * 1e9, 32997, 145 + 330); - EXPECT_NEAR(get_mag_strength_tesla(10, -70) * 1e9, 32389, 145 + 324); - EXPECT_NEAR(get_mag_strength_tesla(10, -65) * 1e9, 31752, 145 + 318); - EXPECT_NEAR(get_mag_strength_tesla(10, -60) * 1e9, 31140, 145 + 311); - EXPECT_NEAR(get_mag_strength_tesla(10, -55) * 1e9, 30613, 145 + 306); - EXPECT_NEAR(get_mag_strength_tesla(10, -50) * 1e9, 30226, 145 + 302); - EXPECT_NEAR(get_mag_strength_tesla(10, -45) * 1e9, 30020, 145 + 300); - EXPECT_NEAR(get_mag_strength_tesla(10, -40) * 1e9, 30003, 145 + 300); - EXPECT_NEAR(get_mag_strength_tesla(10, -35) * 1e9, 30156, 145 + 302); - EXPECT_NEAR(get_mag_strength_tesla(10, -30) * 1e9, 30441, 145 + 304); - EXPECT_NEAR(get_mag_strength_tesla(10, -25) * 1e9, 30820, 145 + 308); - EXPECT_NEAR(get_mag_strength_tesla(10, -20) * 1e9, 31262, 145 + 313); - EXPECT_NEAR(get_mag_strength_tesla(10, -15) * 1e9, 31743, 145 + 317); - EXPECT_NEAR(get_mag_strength_tesla(10, -10) * 1e9, 32248, 145 + 322); - EXPECT_NEAR(get_mag_strength_tesla(10, -5) * 1e9, 32756, 145 + 328); - EXPECT_NEAR(get_mag_strength_tesla(10, 0) * 1e9, 33247, 145 + 332); - EXPECT_NEAR(get_mag_strength_tesla(10, 5) * 1e9, 33697, 145 + 337); - EXPECT_NEAR(get_mag_strength_tesla(10, 10) * 1e9, 34091, 145 + 341); - EXPECT_NEAR(get_mag_strength_tesla(10, 15) * 1e9, 34427, 145 + 344); - EXPECT_NEAR(get_mag_strength_tesla(10, 20) * 1e9, 34725, 145 + 347); - EXPECT_NEAR(get_mag_strength_tesla(10, 25) * 1e9, 35020, 145 + 350); - EXPECT_NEAR(get_mag_strength_tesla(10, 30) * 1e9, 35354, 145 + 354); - EXPECT_NEAR(get_mag_strength_tesla(10, 35) * 1e9, 35762, 145 + 358); - EXPECT_NEAR(get_mag_strength_tesla(10, 40) * 1e9, 36254, 145 + 363); - EXPECT_NEAR(get_mag_strength_tesla(10, 45) * 1e9, 36825, 145 + 368); - EXPECT_NEAR(get_mag_strength_tesla(10, 50) * 1e9, 37458, 145 + 375); - EXPECT_NEAR(get_mag_strength_tesla(10, 55) * 1e9, 38131, 145 + 381); - EXPECT_NEAR(get_mag_strength_tesla(10, 60) * 1e9, 38830, 145 + 388); - EXPECT_NEAR(get_mag_strength_tesla(10, 65) * 1e9, 39537, 145 + 395); - EXPECT_NEAR(get_mag_strength_tesla(10, 70) * 1e9, 40228, 145 + 402); - EXPECT_NEAR(get_mag_strength_tesla(10, 75) * 1e9, 40869, 145 + 409); - EXPECT_NEAR(get_mag_strength_tesla(10, 80) * 1e9, 41422, 145 + 414); - EXPECT_NEAR(get_mag_strength_tesla(10, 85) * 1e9, 41846, 145 + 418); - EXPECT_NEAR(get_mag_strength_tesla(10, 90) * 1e9, 42112, 145 + 421); - EXPECT_NEAR(get_mag_strength_tesla(10, 95) * 1e9, 42201, 145 + 422); - EXPECT_NEAR(get_mag_strength_tesla(10, 100) * 1e9, 42116, 145 + 421); - EXPECT_NEAR(get_mag_strength_tesla(10, 105) * 1e9, 41870, 145 + 419); - EXPECT_NEAR(get_mag_strength_tesla(10, 110) * 1e9, 41486, 145 + 415); - EXPECT_NEAR(get_mag_strength_tesla(10, 115) * 1e9, 40983, 145 + 410); - EXPECT_NEAR(get_mag_strength_tesla(10, 120) * 1e9, 40381, 145 + 404); - EXPECT_NEAR(get_mag_strength_tesla(10, 125) * 1e9, 39695, 145 + 397); - EXPECT_NEAR(get_mag_strength_tesla(10, 130) * 1e9, 38943, 145 + 389); - EXPECT_NEAR(get_mag_strength_tesla(10, 135) * 1e9, 38144, 145 + 381); - EXPECT_NEAR(get_mag_strength_tesla(10, 140) * 1e9, 37324, 145 + 373); - EXPECT_NEAR(get_mag_strength_tesla(10, 145) * 1e9, 36508, 145 + 365); - EXPECT_NEAR(get_mag_strength_tesla(10, 150) * 1e9, 35727, 145 + 357); - EXPECT_NEAR(get_mag_strength_tesla(10, 155) * 1e9, 35006, 145 + 350); - EXPECT_NEAR(get_mag_strength_tesla(10, 160) * 1e9, 34367, 145 + 344); - EXPECT_NEAR(get_mag_strength_tesla(10, 165) * 1e9, 33826, 145 + 338); + EXPECT_NEAR(get_mag_strength_tesla(10, -175) * 1e9, 32644, 145 + 326); + EXPECT_NEAR(get_mag_strength_tesla(10, -170) * 1e9, 32502, 145 + 325); + EXPECT_NEAR(get_mag_strength_tesla(10, -165) * 1e9, 32382, 145 + 324); + EXPECT_NEAR(get_mag_strength_tesla(10, -160) * 1e9, 32287, 145 + 323); + EXPECT_NEAR(get_mag_strength_tesla(10, -155) * 1e9, 32233, 145 + 322); + EXPECT_NEAR(get_mag_strength_tesla(10, -150) * 1e9, 32240, 145 + 322); + EXPECT_NEAR(get_mag_strength_tesla(10, -145) * 1e9, 32322, 145 + 323); + EXPECT_NEAR(get_mag_strength_tesla(10, -140) * 1e9, 32476, 145 + 325); + EXPECT_NEAR(get_mag_strength_tesla(10, -135) * 1e9, 32690, 145 + 327); + EXPECT_NEAR(get_mag_strength_tesla(10, -130) * 1e9, 32941, 145 + 329); + EXPECT_NEAR(get_mag_strength_tesla(10, -125) * 1e9, 33210, 145 + 332); + EXPECT_NEAR(get_mag_strength_tesla(10, -120) * 1e9, 33482, 145 + 335); + EXPECT_NEAR(get_mag_strength_tesla(10, -115) * 1e9, 33748, 145 + 337); + EXPECT_NEAR(get_mag_strength_tesla(10, -110) * 1e9, 33993, 145 + 340); + EXPECT_NEAR(get_mag_strength_tesla(10, -105) * 1e9, 34195, 145 + 342); + EXPECT_NEAR(get_mag_strength_tesla(10, -100) * 1e9, 34321, 145 + 343); + EXPECT_NEAR(get_mag_strength_tesla(10, -95) * 1e9, 34336, 145 + 343); + EXPECT_NEAR(get_mag_strength_tesla(10, -90) * 1e9, 34210, 145 + 342); + EXPECT_NEAR(get_mag_strength_tesla(10, -85) * 1e9, 33934, 145 + 339); + EXPECT_NEAR(get_mag_strength_tesla(10, -80) * 1e9, 33516, 145 + 335); + EXPECT_NEAR(get_mag_strength_tesla(10, -75) * 1e9, 32985, 145 + 330); + EXPECT_NEAR(get_mag_strength_tesla(10, -70) * 1e9, 32377, 145 + 324); + EXPECT_NEAR(get_mag_strength_tesla(10, -65) * 1e9, 31741, 145 + 317); + EXPECT_NEAR(get_mag_strength_tesla(10, -60) * 1e9, 31130, 145 + 311); + EXPECT_NEAR(get_mag_strength_tesla(10, -55) * 1e9, 30604, 145 + 306); + EXPECT_NEAR(get_mag_strength_tesla(10, -50) * 1e9, 30220, 145 + 302); + EXPECT_NEAR(get_mag_strength_tesla(10, -45) * 1e9, 30016, 145 + 300); + EXPECT_NEAR(get_mag_strength_tesla(10, -40) * 1e9, 30001, 145 + 300); + EXPECT_NEAR(get_mag_strength_tesla(10, -35) * 1e9, 30155, 145 + 302); + EXPECT_NEAR(get_mag_strength_tesla(10, -30) * 1e9, 30442, 145 + 304); + EXPECT_NEAR(get_mag_strength_tesla(10, -25) * 1e9, 30822, 145 + 308); + EXPECT_NEAR(get_mag_strength_tesla(10, -20) * 1e9, 31264, 145 + 313); + EXPECT_NEAR(get_mag_strength_tesla(10, -15) * 1e9, 31746, 145 + 317); + EXPECT_NEAR(get_mag_strength_tesla(10, -10) * 1e9, 32250, 145 + 322); + EXPECT_NEAR(get_mag_strength_tesla(10, -5) * 1e9, 32759, 145 + 328); + EXPECT_NEAR(get_mag_strength_tesla(10, 0) * 1e9, 33249, 145 + 332); + EXPECT_NEAR(get_mag_strength_tesla(10, 5) * 1e9, 33699, 145 + 337); + EXPECT_NEAR(get_mag_strength_tesla(10, 10) * 1e9, 34092, 145 + 341); + EXPECT_NEAR(get_mag_strength_tesla(10, 15) * 1e9, 34428, 145 + 344); + EXPECT_NEAR(get_mag_strength_tesla(10, 20) * 1e9, 34726, 145 + 347); + EXPECT_NEAR(get_mag_strength_tesla(10, 25) * 1e9, 35021, 145 + 350); + EXPECT_NEAR(get_mag_strength_tesla(10, 30) * 1e9, 35356, 145 + 354); + EXPECT_NEAR(get_mag_strength_tesla(10, 35) * 1e9, 35764, 145 + 358); + EXPECT_NEAR(get_mag_strength_tesla(10, 40) * 1e9, 36258, 145 + 363); + EXPECT_NEAR(get_mag_strength_tesla(10, 45) * 1e9, 36830, 145 + 368); + EXPECT_NEAR(get_mag_strength_tesla(10, 50) * 1e9, 37463, 145 + 375); + EXPECT_NEAR(get_mag_strength_tesla(10, 55) * 1e9, 38138, 145 + 381); + EXPECT_NEAR(get_mag_strength_tesla(10, 60) * 1e9, 38838, 145 + 388); + EXPECT_NEAR(get_mag_strength_tesla(10, 65) * 1e9, 39546, 145 + 395); + EXPECT_NEAR(get_mag_strength_tesla(10, 70) * 1e9, 40237, 145 + 402); + EXPECT_NEAR(get_mag_strength_tesla(10, 75) * 1e9, 40878, 145 + 409); + EXPECT_NEAR(get_mag_strength_tesla(10, 80) * 1e9, 41431, 145 + 414); + EXPECT_NEAR(get_mag_strength_tesla(10, 85) * 1e9, 41855, 145 + 419); + EXPECT_NEAR(get_mag_strength_tesla(10, 90) * 1e9, 42120, 145 + 421); + EXPECT_NEAR(get_mag_strength_tesla(10, 95) * 1e9, 42208, 145 + 422); + EXPECT_NEAR(get_mag_strength_tesla(10, 100) * 1e9, 42122, 145 + 421); + EXPECT_NEAR(get_mag_strength_tesla(10, 105) * 1e9, 41875, 145 + 419); + EXPECT_NEAR(get_mag_strength_tesla(10, 110) * 1e9, 41490, 145 + 415); + EXPECT_NEAR(get_mag_strength_tesla(10, 115) * 1e9, 40987, 145 + 410); + EXPECT_NEAR(get_mag_strength_tesla(10, 120) * 1e9, 40384, 145 + 404); + EXPECT_NEAR(get_mag_strength_tesla(10, 125) * 1e9, 39698, 145 + 397); + EXPECT_NEAR(get_mag_strength_tesla(10, 130) * 1e9, 38946, 145 + 389); + EXPECT_NEAR(get_mag_strength_tesla(10, 135) * 1e9, 38147, 145 + 381); + EXPECT_NEAR(get_mag_strength_tesla(10, 140) * 1e9, 37326, 145 + 373); + EXPECT_NEAR(get_mag_strength_tesla(10, 145) * 1e9, 36511, 145 + 365); + EXPECT_NEAR(get_mag_strength_tesla(10, 150) * 1e9, 35730, 145 + 357); + EXPECT_NEAR(get_mag_strength_tesla(10, 155) * 1e9, 35008, 145 + 350); + EXPECT_NEAR(get_mag_strength_tesla(10, 160) * 1e9, 34369, 145 + 344); + EXPECT_NEAR(get_mag_strength_tesla(10, 165) * 1e9, 33827, 145 + 338); EXPECT_NEAR(get_mag_strength_tesla(10, 170) * 1e9, 33393, 145 + 334); EXPECT_NEAR(get_mag_strength_tesla(10, 175) * 1e9, 33064, 145 + 331); EXPECT_NEAR(get_mag_strength_tesla(10, 180) * 1e9, 32823, 145 + 328); EXPECT_NEAR(get_mag_strength_tesla(15, -180) * 1e9, 33125, 145 + 331); - EXPECT_NEAR(get_mag_strength_tesla(15, -175) * 1e9, 33045, 145 + 330); - EXPECT_NEAR(get_mag_strength_tesla(15, -170) * 1e9, 33016, 145 + 330); - EXPECT_NEAR(get_mag_strength_tesla(15, -165) * 1e9, 33018, 145 + 330); - EXPECT_NEAR(get_mag_strength_tesla(15, -160) * 1e9, 33056, 145 + 331); - EXPECT_NEAR(get_mag_strength_tesla(15, -155) * 1e9, 33146, 145 + 331); - EXPECT_NEAR(get_mag_strength_tesla(15, -150) * 1e9, 33311, 145 + 333); - EXPECT_NEAR(get_mag_strength_tesla(15, -145) * 1e9, 33563, 145 + 336); - EXPECT_NEAR(get_mag_strength_tesla(15, -140) * 1e9, 33901, 145 + 339); - EXPECT_NEAR(get_mag_strength_tesla(15, -135) * 1e9, 34308, 145 + 343); - EXPECT_NEAR(get_mag_strength_tesla(15, -130) * 1e9, 34757, 145 + 348); - EXPECT_NEAR(get_mag_strength_tesla(15, -125) * 1e9, 35223, 145 + 352); - EXPECT_NEAR(get_mag_strength_tesla(15, -120) * 1e9, 35686, 145 + 357); - EXPECT_NEAR(get_mag_strength_tesla(15, -115) * 1e9, 36131, 145 + 361); - EXPECT_NEAR(get_mag_strength_tesla(15, -110) * 1e9, 36538, 145 + 365); - EXPECT_NEAR(get_mag_strength_tesla(15, -105) * 1e9, 36879, 145 + 369); - EXPECT_NEAR(get_mag_strength_tesla(15, -100) * 1e9, 37116, 145 + 371); - EXPECT_NEAR(get_mag_strength_tesla(15, -95) * 1e9, 37209, 145 + 372); - EXPECT_NEAR(get_mag_strength_tesla(15, -90) * 1e9, 37127, 145 + 371); - EXPECT_NEAR(get_mag_strength_tesla(15, -85) * 1e9, 36859, 145 + 369); - EXPECT_NEAR(get_mag_strength_tesla(15, -80) * 1e9, 36414, 145 + 364); - EXPECT_NEAR(get_mag_strength_tesla(15, -75) * 1e9, 35824, 145 + 358); - EXPECT_NEAR(get_mag_strength_tesla(15, -70) * 1e9, 35132, 145 + 351); - EXPECT_NEAR(get_mag_strength_tesla(15, -65) * 1e9, 34391, 145 + 344); - EXPECT_NEAR(get_mag_strength_tesla(15, -60) * 1e9, 33664, 145 + 337); - EXPECT_NEAR(get_mag_strength_tesla(15, -55) * 1e9, 33013, 145 + 330); - EXPECT_NEAR(get_mag_strength_tesla(15, -50) * 1e9, 32495, 145 + 325); - EXPECT_NEAR(get_mag_strength_tesla(15, -45) * 1e9, 32149, 145 + 321); - EXPECT_NEAR(get_mag_strength_tesla(15, -40) * 1e9, 31986, 145 + 320); - EXPECT_NEAR(get_mag_strength_tesla(15, -35) * 1e9, 31989, 145 + 320); + EXPECT_NEAR(get_mag_strength_tesla(15, -175) * 1e9, 33044, 145 + 330); + EXPECT_NEAR(get_mag_strength_tesla(15, -170) * 1e9, 33014, 145 + 330); + EXPECT_NEAR(get_mag_strength_tesla(15, -165) * 1e9, 33016, 145 + 330); + EXPECT_NEAR(get_mag_strength_tesla(15, -160) * 1e9, 33053, 145 + 331); + EXPECT_NEAR(get_mag_strength_tesla(15, -155) * 1e9, 33143, 145 + 331); + EXPECT_NEAR(get_mag_strength_tesla(15, -150) * 1e9, 33307, 145 + 333); + EXPECT_NEAR(get_mag_strength_tesla(15, -145) * 1e9, 33558, 145 + 336); + EXPECT_NEAR(get_mag_strength_tesla(15, -140) * 1e9, 33896, 145 + 339); + EXPECT_NEAR(get_mag_strength_tesla(15, -135) * 1e9, 34302, 145 + 343); + EXPECT_NEAR(get_mag_strength_tesla(15, -130) * 1e9, 34750, 145 + 348); + EXPECT_NEAR(get_mag_strength_tesla(15, -125) * 1e9, 35216, 145 + 352); + EXPECT_NEAR(get_mag_strength_tesla(15, -120) * 1e9, 35678, 145 + 357); + EXPECT_NEAR(get_mag_strength_tesla(15, -115) * 1e9, 36122, 145 + 361); + EXPECT_NEAR(get_mag_strength_tesla(15, -110) * 1e9, 36528, 145 + 365); + EXPECT_NEAR(get_mag_strength_tesla(15, -105) * 1e9, 36868, 145 + 369); + EXPECT_NEAR(get_mag_strength_tesla(15, -100) * 1e9, 37104, 145 + 371); + EXPECT_NEAR(get_mag_strength_tesla(15, -95) * 1e9, 37196, 145 + 372); + EXPECT_NEAR(get_mag_strength_tesla(15, -90) * 1e9, 37114, 145 + 371); + EXPECT_NEAR(get_mag_strength_tesla(15, -85) * 1e9, 36845, 145 + 368); + EXPECT_NEAR(get_mag_strength_tesla(15, -80) * 1e9, 36401, 145 + 364); + EXPECT_NEAR(get_mag_strength_tesla(15, -75) * 1e9, 35810, 145 + 358); + EXPECT_NEAR(get_mag_strength_tesla(15, -70) * 1e9, 35118, 145 + 351); + EXPECT_NEAR(get_mag_strength_tesla(15, -65) * 1e9, 34379, 145 + 344); + EXPECT_NEAR(get_mag_strength_tesla(15, -60) * 1e9, 33653, 145 + 337); + EXPECT_NEAR(get_mag_strength_tesla(15, -55) * 1e9, 33003, 145 + 330); + EXPECT_NEAR(get_mag_strength_tesla(15, -50) * 1e9, 32487, 145 + 325); + EXPECT_NEAR(get_mag_strength_tesla(15, -45) * 1e9, 32144, 145 + 321); + EXPECT_NEAR(get_mag_strength_tesla(15, -40) * 1e9, 31982, 145 + 320); + EXPECT_NEAR(get_mag_strength_tesla(15, -35) * 1e9, 31987, 145 + 320); EXPECT_NEAR(get_mag_strength_tesla(15, -30) * 1e9, 32129, 145 + 321); EXPECT_NEAR(get_mag_strength_tesla(15, -25) * 1e9, 32375, 145 + 324); - EXPECT_NEAR(get_mag_strength_tesla(15, -20) * 1e9, 32704, 145 + 327); - EXPECT_NEAR(get_mag_strength_tesla(15, -15) * 1e9, 33102, 145 + 331); - EXPECT_NEAR(get_mag_strength_tesla(15, -10) * 1e9, 33554, 145 + 336); - EXPECT_NEAR(get_mag_strength_tesla(15, -5) * 1e9, 34041, 145 + 340); - EXPECT_NEAR(get_mag_strength_tesla(15, 0) * 1e9, 34535, 145 + 345); - EXPECT_NEAR(get_mag_strength_tesla(15, 5) * 1e9, 35010, 145 + 350); - EXPECT_NEAR(get_mag_strength_tesla(15, 10) * 1e9, 35449, 145 + 354); - EXPECT_NEAR(get_mag_strength_tesla(15, 15) * 1e9, 35854, 145 + 359); - EXPECT_NEAR(get_mag_strength_tesla(15, 20) * 1e9, 36242, 145 + 362); - EXPECT_NEAR(get_mag_strength_tesla(15, 25) * 1e9, 36644, 145 + 366); - EXPECT_NEAR(get_mag_strength_tesla(15, 30) * 1e9, 37086, 145 + 371); - EXPECT_NEAR(get_mag_strength_tesla(15, 35) * 1e9, 37578, 145 + 376); - EXPECT_NEAR(get_mag_strength_tesla(15, 40) * 1e9, 38113, 145 + 381); - EXPECT_NEAR(get_mag_strength_tesla(15, 45) * 1e9, 38676, 145 + 387); - EXPECT_NEAR(get_mag_strength_tesla(15, 50) * 1e9, 39252, 145 + 393); - EXPECT_NEAR(get_mag_strength_tesla(15, 55) * 1e9, 39838, 145 + 398); - EXPECT_NEAR(get_mag_strength_tesla(15, 60) * 1e9, 40436, 145 + 404); - EXPECT_NEAR(get_mag_strength_tesla(15, 65) * 1e9, 41042, 145 + 410); - EXPECT_NEAR(get_mag_strength_tesla(15, 70) * 1e9, 41642, 145 + 416); - EXPECT_NEAR(get_mag_strength_tesla(15, 75) * 1e9, 42206, 145 + 422); - EXPECT_NEAR(get_mag_strength_tesla(15, 80) * 1e9, 42698, 145 + 427); - EXPECT_NEAR(get_mag_strength_tesla(15, 85) * 1e9, 43081, 145 + 431); - EXPECT_NEAR(get_mag_strength_tesla(15, 90) * 1e9, 43329, 145 + 433); - EXPECT_NEAR(get_mag_strength_tesla(15, 95) * 1e9, 43426, 145 + 434); - EXPECT_NEAR(get_mag_strength_tesla(15, 100) * 1e9, 43363, 145 + 434); - EXPECT_NEAR(get_mag_strength_tesla(15, 105) * 1e9, 43139, 145 + 431); - EXPECT_NEAR(get_mag_strength_tesla(15, 110) * 1e9, 42753, 145 + 428); - EXPECT_NEAR(get_mag_strength_tesla(15, 115) * 1e9, 42206, 145 + 422); - EXPECT_NEAR(get_mag_strength_tesla(15, 120) * 1e9, 41508, 145 + 415); - EXPECT_NEAR(get_mag_strength_tesla(15, 125) * 1e9, 40683, 145 + 407); - EXPECT_NEAR(get_mag_strength_tesla(15, 130) * 1e9, 39764, 145 + 398); - EXPECT_NEAR(get_mag_strength_tesla(15, 135) * 1e9, 38793, 145 + 388); - EXPECT_NEAR(get_mag_strength_tesla(15, 140) * 1e9, 37813, 145 + 378); - EXPECT_NEAR(get_mag_strength_tesla(15, 145) * 1e9, 36865, 145 + 369); - EXPECT_NEAR(get_mag_strength_tesla(15, 150) * 1e9, 35981, 145 + 360); - EXPECT_NEAR(get_mag_strength_tesla(15, 155) * 1e9, 35190, 145 + 352); - EXPECT_NEAR(get_mag_strength_tesla(15, 160) * 1e9, 34512, 145 + 345); - EXPECT_NEAR(get_mag_strength_tesla(15, 165) * 1e9, 33966, 145 + 340); - EXPECT_NEAR(get_mag_strength_tesla(15, 170) * 1e9, 33559, 145 + 336); + EXPECT_NEAR(get_mag_strength_tesla(15, -20) * 1e9, 32706, 145 + 327); + EXPECT_NEAR(get_mag_strength_tesla(15, -15) * 1e9, 33104, 145 + 331); + EXPECT_NEAR(get_mag_strength_tesla(15, -10) * 1e9, 33557, 145 + 336); + EXPECT_NEAR(get_mag_strength_tesla(15, -5) * 1e9, 34043, 145 + 340); + EXPECT_NEAR(get_mag_strength_tesla(15, 0) * 1e9, 34537, 145 + 345); + EXPECT_NEAR(get_mag_strength_tesla(15, 5) * 1e9, 35012, 145 + 350); + EXPECT_NEAR(get_mag_strength_tesla(15, 10) * 1e9, 35452, 145 + 355); + EXPECT_NEAR(get_mag_strength_tesla(15, 15) * 1e9, 35856, 145 + 359); + EXPECT_NEAR(get_mag_strength_tesla(15, 20) * 1e9, 36245, 145 + 362); + EXPECT_NEAR(get_mag_strength_tesla(15, 25) * 1e9, 36647, 145 + 366); + EXPECT_NEAR(get_mag_strength_tesla(15, 30) * 1e9, 37090, 145 + 371); + EXPECT_NEAR(get_mag_strength_tesla(15, 35) * 1e9, 37582, 145 + 376); + EXPECT_NEAR(get_mag_strength_tesla(15, 40) * 1e9, 38118, 145 + 381); + EXPECT_NEAR(get_mag_strength_tesla(15, 45) * 1e9, 38682, 145 + 387); + EXPECT_NEAR(get_mag_strength_tesla(15, 50) * 1e9, 39259, 145 + 393); + EXPECT_NEAR(get_mag_strength_tesla(15, 55) * 1e9, 39846, 145 + 398); + EXPECT_NEAR(get_mag_strength_tesla(15, 60) * 1e9, 40444, 145 + 404); + EXPECT_NEAR(get_mag_strength_tesla(15, 65) * 1e9, 41051, 145 + 411); + EXPECT_NEAR(get_mag_strength_tesla(15, 70) * 1e9, 41652, 145 + 417); + EXPECT_NEAR(get_mag_strength_tesla(15, 75) * 1e9, 42216, 145 + 422); + EXPECT_NEAR(get_mag_strength_tesla(15, 80) * 1e9, 42708, 145 + 427); + EXPECT_NEAR(get_mag_strength_tesla(15, 85) * 1e9, 43092, 145 + 431); + EXPECT_NEAR(get_mag_strength_tesla(15, 90) * 1e9, 43339, 145 + 433); + EXPECT_NEAR(get_mag_strength_tesla(15, 95) * 1e9, 43434, 145 + 434); + EXPECT_NEAR(get_mag_strength_tesla(15, 100) * 1e9, 43371, 145 + 434); + EXPECT_NEAR(get_mag_strength_tesla(15, 105) * 1e9, 43146, 145 + 431); + EXPECT_NEAR(get_mag_strength_tesla(15, 110) * 1e9, 42759, 145 + 428); + EXPECT_NEAR(get_mag_strength_tesla(15, 115) * 1e9, 42211, 145 + 422); + EXPECT_NEAR(get_mag_strength_tesla(15, 120) * 1e9, 41513, 145 + 415); + EXPECT_NEAR(get_mag_strength_tesla(15, 125) * 1e9, 40687, 145 + 407); + EXPECT_NEAR(get_mag_strength_tesla(15, 130) * 1e9, 39767, 145 + 398); + EXPECT_NEAR(get_mag_strength_tesla(15, 135) * 1e9, 38796, 145 + 388); + EXPECT_NEAR(get_mag_strength_tesla(15, 140) * 1e9, 37816, 145 + 378); + EXPECT_NEAR(get_mag_strength_tesla(15, 145) * 1e9, 36868, 145 + 369); + EXPECT_NEAR(get_mag_strength_tesla(15, 150) * 1e9, 35984, 145 + 360); + EXPECT_NEAR(get_mag_strength_tesla(15, 155) * 1e9, 35192, 145 + 352); + EXPECT_NEAR(get_mag_strength_tesla(15, 160) * 1e9, 34514, 145 + 345); + EXPECT_NEAR(get_mag_strength_tesla(15, 165) * 1e9, 33967, 145 + 340); + EXPECT_NEAR(get_mag_strength_tesla(15, 170) * 1e9, 33560, 145 + 336); EXPECT_NEAR(get_mag_strength_tesla(15, 175) * 1e9, 33286, 145 + 333); EXPECT_NEAR(get_mag_strength_tesla(15, 180) * 1e9, 33125, 145 + 331); EXPECT_NEAR(get_mag_strength_tesla(20, -180) * 1e9, 33989, 145 + 340); - EXPECT_NEAR(get_mag_strength_tesla(20, -175) * 1e9, 33961, 145 + 340); - EXPECT_NEAR(get_mag_strength_tesla(20, -170) * 1e9, 34004, 145 + 340); - EXPECT_NEAR(get_mag_strength_tesla(20, -165) * 1e9, 34101, 145 + 341); - EXPECT_NEAR(get_mag_strength_tesla(20, -160) * 1e9, 34252, 145 + 343); - EXPECT_NEAR(get_mag_strength_tesla(20, -155) * 1e9, 34470, 145 + 345); - EXPECT_NEAR(get_mag_strength_tesla(20, -150) * 1e9, 34777, 145 + 348); - EXPECT_NEAR(get_mag_strength_tesla(20, -145) * 1e9, 35183, 145 + 352); - EXPECT_NEAR(get_mag_strength_tesla(20, -140) * 1e9, 35685, 145 + 357); - EXPECT_NEAR(get_mag_strength_tesla(20, -135) * 1e9, 36261, 145 + 363); - EXPECT_NEAR(get_mag_strength_tesla(20, -130) * 1e9, 36884, 145 + 369); - EXPECT_NEAR(get_mag_strength_tesla(20, -125) * 1e9, 37524, 145 + 375); - EXPECT_NEAR(get_mag_strength_tesla(20, -120) * 1e9, 38157, 145 + 382); - EXPECT_NEAR(get_mag_strength_tesla(20, -115) * 1e9, 38761, 145 + 388); - EXPECT_NEAR(get_mag_strength_tesla(20, -110) * 1e9, 39310, 145 + 393); - EXPECT_NEAR(get_mag_strength_tesla(20, -105) * 1e9, 39772, 145 + 398); - EXPECT_NEAR(get_mag_strength_tesla(20, -100) * 1e9, 40103, 145 + 401); - EXPECT_NEAR(get_mag_strength_tesla(20, -95) * 1e9, 40262, 145 + 403); - EXPECT_NEAR(get_mag_strength_tesla(20, -90) * 1e9, 40216, 145 + 402); - EXPECT_NEAR(get_mag_strength_tesla(20, -85) * 1e9, 39951, 145 + 400); - EXPECT_NEAR(get_mag_strength_tesla(20, -80) * 1e9, 39479, 145 + 395); - EXPECT_NEAR(get_mag_strength_tesla(20, -75) * 1e9, 38834, 145 + 388); - EXPECT_NEAR(get_mag_strength_tesla(20, -70) * 1e9, 38066, 145 + 381); - EXPECT_NEAR(get_mag_strength_tesla(20, -65) * 1e9, 37236, 145 + 372); - EXPECT_NEAR(get_mag_strength_tesla(20, -60) * 1e9, 36414, 145 + 364); - EXPECT_NEAR(get_mag_strength_tesla(20, -55) * 1e9, 35666, 145 + 357); - EXPECT_NEAR(get_mag_strength_tesla(20, -50) * 1e9, 35050, 145 + 351); - EXPECT_NEAR(get_mag_strength_tesla(20, -45) * 1e9, 34601, 145 + 346); - EXPECT_NEAR(get_mag_strength_tesla(20, -40) * 1e9, 34326, 145 + 343); - EXPECT_NEAR(get_mag_strength_tesla(20, -35) * 1e9, 34212, 145 + 342); - EXPECT_NEAR(get_mag_strength_tesla(20, -30) * 1e9, 34233, 145 + 342); + EXPECT_NEAR(get_mag_strength_tesla(20, -175) * 1e9, 33959, 145 + 340); + EXPECT_NEAR(get_mag_strength_tesla(20, -170) * 1e9, 34002, 145 + 340); + EXPECT_NEAR(get_mag_strength_tesla(20, -165) * 1e9, 34098, 145 + 341); + EXPECT_NEAR(get_mag_strength_tesla(20, -160) * 1e9, 34248, 145 + 342); + EXPECT_NEAR(get_mag_strength_tesla(20, -155) * 1e9, 34466, 145 + 345); + EXPECT_NEAR(get_mag_strength_tesla(20, -150) * 1e9, 34772, 145 + 348); + EXPECT_NEAR(get_mag_strength_tesla(20, -145) * 1e9, 35178, 145 + 352); + EXPECT_NEAR(get_mag_strength_tesla(20, -140) * 1e9, 35678, 145 + 357); + EXPECT_NEAR(get_mag_strength_tesla(20, -135) * 1e9, 36254, 145 + 363); + EXPECT_NEAR(get_mag_strength_tesla(20, -130) * 1e9, 36876, 145 + 369); + EXPECT_NEAR(get_mag_strength_tesla(20, -125) * 1e9, 37516, 145 + 375); + EXPECT_NEAR(get_mag_strength_tesla(20, -120) * 1e9, 38148, 145 + 381); + EXPECT_NEAR(get_mag_strength_tesla(20, -115) * 1e9, 38751, 145 + 388); + EXPECT_NEAR(get_mag_strength_tesla(20, -110) * 1e9, 39299, 145 + 393); + EXPECT_NEAR(get_mag_strength_tesla(20, -105) * 1e9, 39760, 145 + 398); + EXPECT_NEAR(get_mag_strength_tesla(20, -100) * 1e9, 40090, 145 + 401); + EXPECT_NEAR(get_mag_strength_tesla(20, -95) * 1e9, 40248, 145 + 402); + EXPECT_NEAR(get_mag_strength_tesla(20, -90) * 1e9, 40201, 145 + 402); + EXPECT_NEAR(get_mag_strength_tesla(20, -85) * 1e9, 39936, 145 + 399); + EXPECT_NEAR(get_mag_strength_tesla(20, -80) * 1e9, 39465, 145 + 395); + EXPECT_NEAR(get_mag_strength_tesla(20, -75) * 1e9, 38820, 145 + 388); + EXPECT_NEAR(get_mag_strength_tesla(20, -70) * 1e9, 38052, 145 + 381); + EXPECT_NEAR(get_mag_strength_tesla(20, -65) * 1e9, 37223, 145 + 372); + EXPECT_NEAR(get_mag_strength_tesla(20, -60) * 1e9, 36402, 145 + 364); + EXPECT_NEAR(get_mag_strength_tesla(20, -55) * 1e9, 35656, 145 + 357); + EXPECT_NEAR(get_mag_strength_tesla(20, -50) * 1e9, 35042, 145 + 350); + EXPECT_NEAR(get_mag_strength_tesla(20, -45) * 1e9, 34595, 145 + 346); + EXPECT_NEAR(get_mag_strength_tesla(20, -40) * 1e9, 34322, 145 + 343); + EXPECT_NEAR(get_mag_strength_tesla(20, -35) * 1e9, 34210, 145 + 342); + EXPECT_NEAR(get_mag_strength_tesla(20, -30) * 1e9, 34232, 145 + 342); EXPECT_NEAR(get_mag_strength_tesla(20, -25) * 1e9, 34368, 145 + 344); - EXPECT_NEAR(get_mag_strength_tesla(20, -20) * 1e9, 34604, 145 + 346); - EXPECT_NEAR(get_mag_strength_tesla(20, -15) * 1e9, 34934, 145 + 349); - EXPECT_NEAR(get_mag_strength_tesla(20, -10) * 1e9, 35345, 145 + 353); - EXPECT_NEAR(get_mag_strength_tesla(20, -5) * 1e9, 35817, 145 + 358); - EXPECT_NEAR(get_mag_strength_tesla(20, 0) * 1e9, 36318, 145 + 363); - EXPECT_NEAR(get_mag_strength_tesla(20, 5) * 1e9, 36817, 145 + 368); - EXPECT_NEAR(get_mag_strength_tesla(20, 10) * 1e9, 37294, 145 + 373); - EXPECT_NEAR(get_mag_strength_tesla(20, 15) * 1e9, 37751, 145 + 378); - EXPECT_NEAR(get_mag_strength_tesla(20, 20) * 1e9, 38206, 145 + 382); - EXPECT_NEAR(get_mag_strength_tesla(20, 25) * 1e9, 38684, 145 + 387); - EXPECT_NEAR(get_mag_strength_tesla(20, 30) * 1e9, 39200, 145 + 392); - EXPECT_NEAR(get_mag_strength_tesla(20, 35) * 1e9, 39752, 145 + 398); - EXPECT_NEAR(get_mag_strength_tesla(20, 40) * 1e9, 40322, 145 + 403); - EXPECT_NEAR(get_mag_strength_tesla(20, 45) * 1e9, 40890, 145 + 409); - EXPECT_NEAR(get_mag_strength_tesla(20, 50) * 1e9, 41445, 145 + 414); - EXPECT_NEAR(get_mag_strength_tesla(20, 55) * 1e9, 41994, 145 + 420); - EXPECT_NEAR(get_mag_strength_tesla(20, 60) * 1e9, 42547, 145 + 425); - EXPECT_NEAR(get_mag_strength_tesla(20, 65) * 1e9, 43112, 145 + 431); - EXPECT_NEAR(get_mag_strength_tesla(20, 70) * 1e9, 43680, 145 + 437); - EXPECT_NEAR(get_mag_strength_tesla(20, 75) * 1e9, 44225, 145 + 442); - EXPECT_NEAR(get_mag_strength_tesla(20, 80) * 1e9, 44708, 145 + 447); - EXPECT_NEAR(get_mag_strength_tesla(20, 85) * 1e9, 45097, 145 + 451); - EXPECT_NEAR(get_mag_strength_tesla(20, 90) * 1e9, 45362, 145 + 454); - EXPECT_NEAR(get_mag_strength_tesla(20, 95) * 1e9, 45485, 145 + 455); - EXPECT_NEAR(get_mag_strength_tesla(20, 100) * 1e9, 45450, 145 + 455); - EXPECT_NEAR(get_mag_strength_tesla(20, 105) * 1e9, 45243, 145 + 452); - EXPECT_NEAR(get_mag_strength_tesla(20, 110) * 1e9, 44846, 145 + 448); - EXPECT_NEAR(get_mag_strength_tesla(20, 115) * 1e9, 44251, 145 + 443); - EXPECT_NEAR(get_mag_strength_tesla(20, 120) * 1e9, 43462, 145 + 435); - EXPECT_NEAR(get_mag_strength_tesla(20, 125) * 1e9, 42505, 145 + 425); - EXPECT_NEAR(get_mag_strength_tesla(20, 130) * 1e9, 41429, 145 + 414); - EXPECT_NEAR(get_mag_strength_tesla(20, 135) * 1e9, 40290, 145 + 403); - EXPECT_NEAR(get_mag_strength_tesla(20, 140) * 1e9, 39147, 145 + 391); - EXPECT_NEAR(get_mag_strength_tesla(20, 145) * 1e9, 38053, 145 + 381); - EXPECT_NEAR(get_mag_strength_tesla(20, 150) * 1e9, 37047, 145 + 370); - EXPECT_NEAR(get_mag_strength_tesla(20, 155) * 1e9, 36158, 145 + 362); - EXPECT_NEAR(get_mag_strength_tesla(20, 160) * 1e9, 35410, 145 + 354); - EXPECT_NEAR(get_mag_strength_tesla(20, 165) * 1e9, 34818, 145 + 348); - EXPECT_NEAR(get_mag_strength_tesla(20, 170) * 1e9, 34391, 145 + 344); + EXPECT_NEAR(get_mag_strength_tesla(20, -20) * 1e9, 34605, 145 + 346); + EXPECT_NEAR(get_mag_strength_tesla(20, -15) * 1e9, 34935, 145 + 349); + EXPECT_NEAR(get_mag_strength_tesla(20, -10) * 1e9, 35348, 145 + 353); + EXPECT_NEAR(get_mag_strength_tesla(20, -5) * 1e9, 35820, 145 + 358); + EXPECT_NEAR(get_mag_strength_tesla(20, 0) * 1e9, 36322, 145 + 363); + EXPECT_NEAR(get_mag_strength_tesla(20, 5) * 1e9, 36821, 145 + 368); + EXPECT_NEAR(get_mag_strength_tesla(20, 10) * 1e9, 37298, 145 + 373); + EXPECT_NEAR(get_mag_strength_tesla(20, 15) * 1e9, 37755, 145 + 378); + EXPECT_NEAR(get_mag_strength_tesla(20, 20) * 1e9, 38211, 145 + 382); + EXPECT_NEAR(get_mag_strength_tesla(20, 25) * 1e9, 38689, 145 + 387); + EXPECT_NEAR(get_mag_strength_tesla(20, 30) * 1e9, 39206, 145 + 392); + EXPECT_NEAR(get_mag_strength_tesla(20, 35) * 1e9, 39758, 145 + 398); + EXPECT_NEAR(get_mag_strength_tesla(20, 40) * 1e9, 40329, 145 + 403); + EXPECT_NEAR(get_mag_strength_tesla(20, 45) * 1e9, 40897, 145 + 409); + EXPECT_NEAR(get_mag_strength_tesla(20, 50) * 1e9, 41453, 145 + 415); + EXPECT_NEAR(get_mag_strength_tesla(20, 55) * 1e9, 42002, 145 + 420); + EXPECT_NEAR(get_mag_strength_tesla(20, 60) * 1e9, 42556, 145 + 426); + EXPECT_NEAR(get_mag_strength_tesla(20, 65) * 1e9, 43122, 145 + 431); + EXPECT_NEAR(get_mag_strength_tesla(20, 70) * 1e9, 43691, 145 + 437); + EXPECT_NEAR(get_mag_strength_tesla(20, 75) * 1e9, 44236, 145 + 442); + EXPECT_NEAR(get_mag_strength_tesla(20, 80) * 1e9, 44720, 145 + 447); + EXPECT_NEAR(get_mag_strength_tesla(20, 85) * 1e9, 45108, 145 + 451); + EXPECT_NEAR(get_mag_strength_tesla(20, 90) * 1e9, 45373, 145 + 454); + EXPECT_NEAR(get_mag_strength_tesla(20, 95) * 1e9, 45495, 145 + 455); + EXPECT_NEAR(get_mag_strength_tesla(20, 100) * 1e9, 45460, 145 + 455); + EXPECT_NEAR(get_mag_strength_tesla(20, 105) * 1e9, 45251, 145 + 453); + EXPECT_NEAR(get_mag_strength_tesla(20, 110) * 1e9, 44854, 145 + 449); + EXPECT_NEAR(get_mag_strength_tesla(20, 115) * 1e9, 44257, 145 + 443); + EXPECT_NEAR(get_mag_strength_tesla(20, 120) * 1e9, 43467, 145 + 435); + EXPECT_NEAR(get_mag_strength_tesla(20, 125) * 1e9, 42510, 145 + 425); + EXPECT_NEAR(get_mag_strength_tesla(20, 130) * 1e9, 41433, 145 + 414); + EXPECT_NEAR(get_mag_strength_tesla(20, 135) * 1e9, 40293, 145 + 403); + EXPECT_NEAR(get_mag_strength_tesla(20, 140) * 1e9, 39151, 145 + 392); + EXPECT_NEAR(get_mag_strength_tesla(20, 145) * 1e9, 38056, 145 + 381); + EXPECT_NEAR(get_mag_strength_tesla(20, 150) * 1e9, 37049, 145 + 370); + EXPECT_NEAR(get_mag_strength_tesla(20, 155) * 1e9, 36160, 145 + 362); + EXPECT_NEAR(get_mag_strength_tesla(20, 160) * 1e9, 35412, 145 + 354); + EXPECT_NEAR(get_mag_strength_tesla(20, 165) * 1e9, 34820, 145 + 348); + EXPECT_NEAR(get_mag_strength_tesla(20, 170) * 1e9, 34392, 145 + 344); EXPECT_NEAR(get_mag_strength_tesla(20, 175) * 1e9, 34122, 145 + 341); EXPECT_NEAR(get_mag_strength_tesla(20, 180) * 1e9, 33989, 145 + 340); EXPECT_NEAR(get_mag_strength_tesla(25, -180) * 1e9, 35366, 145 + 354); - EXPECT_NEAR(get_mag_strength_tesla(25, -175) * 1e9, 35345, 145 + 353); - EXPECT_NEAR(get_mag_strength_tesla(25, -170) * 1e9, 35425, 145 + 354); - EXPECT_NEAR(get_mag_strength_tesla(25, -165) * 1e9, 35589, 145 + 356); - EXPECT_NEAR(get_mag_strength_tesla(25, -160) * 1e9, 35833, 145 + 358); - EXPECT_NEAR(get_mag_strength_tesla(25, -155) * 1e9, 36166, 145 + 362); - EXPECT_NEAR(get_mag_strength_tesla(25, -150) * 1e9, 36602, 145 + 366); - EXPECT_NEAR(get_mag_strength_tesla(25, -145) * 1e9, 37144, 145 + 371); - EXPECT_NEAR(get_mag_strength_tesla(25, -140) * 1e9, 37786, 145 + 378); - EXPECT_NEAR(get_mag_strength_tesla(25, -135) * 1e9, 38504, 145 + 385); - EXPECT_NEAR(get_mag_strength_tesla(25, -130) * 1e9, 39270, 145 + 393); - EXPECT_NEAR(get_mag_strength_tesla(25, -125) * 1e9, 40051, 145 + 401); - EXPECT_NEAR(get_mag_strength_tesla(25, -120) * 1e9, 40820, 145 + 408); - EXPECT_NEAR(get_mag_strength_tesla(25, -115) * 1e9, 41551, 145 + 416); - EXPECT_NEAR(get_mag_strength_tesla(25, -110) * 1e9, 42214, 145 + 422); - EXPECT_NEAR(get_mag_strength_tesla(25, -105) * 1e9, 42770, 145 + 428); - EXPECT_NEAR(get_mag_strength_tesla(25, -100) * 1e9, 43177, 145 + 432); - EXPECT_NEAR(get_mag_strength_tesla(25, -95) * 1e9, 43390, 145 + 434); - EXPECT_NEAR(get_mag_strength_tesla(25, -90) * 1e9, 43375, 145 + 434); - EXPECT_NEAR(get_mag_strength_tesla(25, -85) * 1e9, 43119, 145 + 431); - EXPECT_NEAR(get_mag_strength_tesla(25, -80) * 1e9, 42632, 145 + 426); - EXPECT_NEAR(get_mag_strength_tesla(25, -75) * 1e9, 41949, 145 + 419); - EXPECT_NEAR(get_mag_strength_tesla(25, -70) * 1e9, 41126, 145 + 411); - EXPECT_NEAR(get_mag_strength_tesla(25, -65) * 1e9, 40231, 145 + 402); - EXPECT_NEAR(get_mag_strength_tesla(25, -60) * 1e9, 39338, 145 + 393); - EXPECT_NEAR(get_mag_strength_tesla(25, -55) * 1e9, 38519, 145 + 385); - EXPECT_NEAR(get_mag_strength_tesla(25, -50) * 1e9, 37831, 145 + 378); - EXPECT_NEAR(get_mag_strength_tesla(25, -45) * 1e9, 37304, 145 + 373); - EXPECT_NEAR(get_mag_strength_tesla(25, -40) * 1e9, 36946, 145 + 369); - EXPECT_NEAR(get_mag_strength_tesla(25, -35) * 1e9, 36741, 145 + 367); - EXPECT_NEAR(get_mag_strength_tesla(25, -30) * 1e9, 36669, 145 + 367); + EXPECT_NEAR(get_mag_strength_tesla(25, -175) * 1e9, 35343, 145 + 353); + EXPECT_NEAR(get_mag_strength_tesla(25, -170) * 1e9, 35423, 145 + 354); + EXPECT_NEAR(get_mag_strength_tesla(25, -165) * 1e9, 35586, 145 + 356); + EXPECT_NEAR(get_mag_strength_tesla(25, -160) * 1e9, 35829, 145 + 358); + EXPECT_NEAR(get_mag_strength_tesla(25, -155) * 1e9, 36161, 145 + 362); + EXPECT_NEAR(get_mag_strength_tesla(25, -150) * 1e9, 36595, 145 + 366); + EXPECT_NEAR(get_mag_strength_tesla(25, -145) * 1e9, 37137, 145 + 371); + EXPECT_NEAR(get_mag_strength_tesla(25, -140) * 1e9, 37778, 145 + 378); + EXPECT_NEAR(get_mag_strength_tesla(25, -135) * 1e9, 38496, 145 + 385); + EXPECT_NEAR(get_mag_strength_tesla(25, -130) * 1e9, 39260, 145 + 393); + EXPECT_NEAR(get_mag_strength_tesla(25, -125) * 1e9, 40041, 145 + 400); + EXPECT_NEAR(get_mag_strength_tesla(25, -120) * 1e9, 40810, 145 + 408); + EXPECT_NEAR(get_mag_strength_tesla(25, -115) * 1e9, 41540, 145 + 415); + EXPECT_NEAR(get_mag_strength_tesla(25, -110) * 1e9, 42201, 145 + 422); + EXPECT_NEAR(get_mag_strength_tesla(25, -105) * 1e9, 42757, 145 + 428); + EXPECT_NEAR(get_mag_strength_tesla(25, -100) * 1e9, 43163, 145 + 432); + EXPECT_NEAR(get_mag_strength_tesla(25, -95) * 1e9, 43375, 145 + 434); + EXPECT_NEAR(get_mag_strength_tesla(25, -90) * 1e9, 43360, 145 + 434); + EXPECT_NEAR(get_mag_strength_tesla(25, -85) * 1e9, 43104, 145 + 431); + EXPECT_NEAR(get_mag_strength_tesla(25, -80) * 1e9, 42617, 145 + 426); + EXPECT_NEAR(get_mag_strength_tesla(25, -75) * 1e9, 41934, 145 + 419); + EXPECT_NEAR(get_mag_strength_tesla(25, -70) * 1e9, 41112, 145 + 411); + EXPECT_NEAR(get_mag_strength_tesla(25, -65) * 1e9, 40217, 145 + 402); + EXPECT_NEAR(get_mag_strength_tesla(25, -60) * 1e9, 39326, 145 + 393); + EXPECT_NEAR(get_mag_strength_tesla(25, -55) * 1e9, 38509, 145 + 385); + EXPECT_NEAR(get_mag_strength_tesla(25, -50) * 1e9, 37822, 145 + 378); + EXPECT_NEAR(get_mag_strength_tesla(25, -45) * 1e9, 37298, 145 + 373); + EXPECT_NEAR(get_mag_strength_tesla(25, -40) * 1e9, 36941, 145 + 369); + EXPECT_NEAR(get_mag_strength_tesla(25, -35) * 1e9, 36738, 145 + 367); + EXPECT_NEAR(get_mag_strength_tesla(25, -30) * 1e9, 36667, 145 + 367); EXPECT_NEAR(get_mag_strength_tesla(25, -25) * 1e9, 36714, 145 + 367); EXPECT_NEAR(get_mag_strength_tesla(25, -20) * 1e9, 36873, 145 + 369); - EXPECT_NEAR(get_mag_strength_tesla(25, -15) * 1e9, 37142, 145 + 371); - EXPECT_NEAR(get_mag_strength_tesla(25, -10) * 1e9, 37513, 145 + 375); - EXPECT_NEAR(get_mag_strength_tesla(25, -5) * 1e9, 37964, 145 + 380); - EXPECT_NEAR(get_mag_strength_tesla(25, 0) * 1e9, 38462, 145 + 385); - EXPECT_NEAR(get_mag_strength_tesla(25, 5) * 1e9, 38971, 145 + 390); - EXPECT_NEAR(get_mag_strength_tesla(25, 10) * 1e9, 39469, 145 + 395); - EXPECT_NEAR(get_mag_strength_tesla(25, 15) * 1e9, 39956, 145 + 400); - EXPECT_NEAR(get_mag_strength_tesla(25, 20) * 1e9, 40446, 145 + 404); - EXPECT_NEAR(get_mag_strength_tesla(25, 25) * 1e9, 40961, 145 + 410); - EXPECT_NEAR(get_mag_strength_tesla(25, 30) * 1e9, 41510, 145 + 415); - EXPECT_NEAR(get_mag_strength_tesla(25, 35) * 1e9, 42089, 145 + 421); - EXPECT_NEAR(get_mag_strength_tesla(25, 40) * 1e9, 42676, 145 + 427); - EXPECT_NEAR(get_mag_strength_tesla(25, 45) * 1e9, 43254, 145 + 433); - EXPECT_NEAR(get_mag_strength_tesla(25, 50) * 1e9, 43815, 145 + 438); - EXPECT_NEAR(get_mag_strength_tesla(25, 55) * 1e9, 44368, 145 + 444); - EXPECT_NEAR(get_mag_strength_tesla(25, 60) * 1e9, 44929, 145 + 449); - EXPECT_NEAR(get_mag_strength_tesla(25, 65) * 1e9, 45507, 145 + 455); - EXPECT_NEAR(get_mag_strength_tesla(25, 70) * 1e9, 46095, 145 + 461); - EXPECT_NEAR(get_mag_strength_tesla(25, 75) * 1e9, 46667, 145 + 467); - EXPECT_NEAR(get_mag_strength_tesla(25, 80) * 1e9, 47187, 145 + 472); - EXPECT_NEAR(get_mag_strength_tesla(25, 85) * 1e9, 47616, 145 + 476); - EXPECT_NEAR(get_mag_strength_tesla(25, 90) * 1e9, 47926, 145 + 479); - EXPECT_NEAR(get_mag_strength_tesla(25, 95) * 1e9, 48090, 145 + 481); - EXPECT_NEAR(get_mag_strength_tesla(25, 100) * 1e9, 48088, 145 + 481); - EXPECT_NEAR(get_mag_strength_tesla(25, 105) * 1e9, 47895, 145 + 479); - EXPECT_NEAR(get_mag_strength_tesla(25, 110) * 1e9, 47486, 145 + 475); - EXPECT_NEAR(get_mag_strength_tesla(25, 115) * 1e9, 46848, 145 + 468); - EXPECT_NEAR(get_mag_strength_tesla(25, 120) * 1e9, 45984, 145 + 460); - EXPECT_NEAR(get_mag_strength_tesla(25, 125) * 1e9, 44923, 145 + 449); - EXPECT_NEAR(get_mag_strength_tesla(25, 130) * 1e9, 43719, 145 + 437); - EXPECT_NEAR(get_mag_strength_tesla(25, 135) * 1e9, 42440, 145 + 424); - EXPECT_NEAR(get_mag_strength_tesla(25, 140) * 1e9, 41157, 145 + 412); - EXPECT_NEAR(get_mag_strength_tesla(25, 145) * 1e9, 39928, 145 + 399); - EXPECT_NEAR(get_mag_strength_tesla(25, 150) * 1e9, 38800, 145 + 388); - EXPECT_NEAR(get_mag_strength_tesla(25, 155) * 1e9, 37807, 145 + 378); - EXPECT_NEAR(get_mag_strength_tesla(25, 160) * 1e9, 36971, 145 + 370); - EXPECT_NEAR(get_mag_strength_tesla(25, 165) * 1e9, 36308, 145 + 363); - EXPECT_NEAR(get_mag_strength_tesla(25, 170) * 1e9, 35825, 145 + 358); + EXPECT_NEAR(get_mag_strength_tesla(25, -15) * 1e9, 37143, 145 + 371); + EXPECT_NEAR(get_mag_strength_tesla(25, -10) * 1e9, 37516, 145 + 375); + EXPECT_NEAR(get_mag_strength_tesla(25, -5) * 1e9, 37968, 145 + 380); + EXPECT_NEAR(get_mag_strength_tesla(25, 0) * 1e9, 38466, 145 + 385); + EXPECT_NEAR(get_mag_strength_tesla(25, 5) * 1e9, 38975, 145 + 390); + EXPECT_NEAR(get_mag_strength_tesla(25, 10) * 1e9, 39474, 145 + 395); + EXPECT_NEAR(get_mag_strength_tesla(25, 15) * 1e9, 39961, 145 + 400); + EXPECT_NEAR(get_mag_strength_tesla(25, 20) * 1e9, 40452, 145 + 405); + EXPECT_NEAR(get_mag_strength_tesla(25, 25) * 1e9, 40967, 145 + 410); + EXPECT_NEAR(get_mag_strength_tesla(25, 30) * 1e9, 41517, 145 + 415); + EXPECT_NEAR(get_mag_strength_tesla(25, 35) * 1e9, 42096, 145 + 421); + EXPECT_NEAR(get_mag_strength_tesla(25, 40) * 1e9, 42684, 145 + 427); + EXPECT_NEAR(get_mag_strength_tesla(25, 45) * 1e9, 43262, 145 + 433); + EXPECT_NEAR(get_mag_strength_tesla(25, 50) * 1e9, 43823, 145 + 438); + EXPECT_NEAR(get_mag_strength_tesla(25, 55) * 1e9, 44377, 145 + 444); + EXPECT_NEAR(get_mag_strength_tesla(25, 60) * 1e9, 44938, 145 + 449); + EXPECT_NEAR(get_mag_strength_tesla(25, 65) * 1e9, 45518, 145 + 455); + EXPECT_NEAR(get_mag_strength_tesla(25, 70) * 1e9, 46107, 145 + 461); + EXPECT_NEAR(get_mag_strength_tesla(25, 75) * 1e9, 46679, 145 + 467); + EXPECT_NEAR(get_mag_strength_tesla(25, 80) * 1e9, 47199, 145 + 472); + EXPECT_NEAR(get_mag_strength_tesla(25, 85) * 1e9, 47629, 145 + 476); + EXPECT_NEAR(get_mag_strength_tesla(25, 90) * 1e9, 47938, 145 + 479); + EXPECT_NEAR(get_mag_strength_tesla(25, 95) * 1e9, 48102, 145 + 481); + EXPECT_NEAR(get_mag_strength_tesla(25, 100) * 1e9, 48098, 145 + 481); + EXPECT_NEAR(get_mag_strength_tesla(25, 105) * 1e9, 47904, 145 + 479); + EXPECT_NEAR(get_mag_strength_tesla(25, 110) * 1e9, 47494, 145 + 475); + EXPECT_NEAR(get_mag_strength_tesla(25, 115) * 1e9, 46855, 145 + 469); + EXPECT_NEAR(get_mag_strength_tesla(25, 120) * 1e9, 45990, 145 + 460); + EXPECT_NEAR(get_mag_strength_tesla(25, 125) * 1e9, 44928, 145 + 449); + EXPECT_NEAR(get_mag_strength_tesla(25, 130) * 1e9, 43724, 145 + 437); + EXPECT_NEAR(get_mag_strength_tesla(25, 135) * 1e9, 42445, 145 + 424); + EXPECT_NEAR(get_mag_strength_tesla(25, 140) * 1e9, 41160, 145 + 412); + EXPECT_NEAR(get_mag_strength_tesla(25, 145) * 1e9, 39931, 145 + 399); + EXPECT_NEAR(get_mag_strength_tesla(25, 150) * 1e9, 38803, 145 + 388); + EXPECT_NEAR(get_mag_strength_tesla(25, 155) * 1e9, 37810, 145 + 378); + EXPECT_NEAR(get_mag_strength_tesla(25, 160) * 1e9, 36973, 145 + 370); + EXPECT_NEAR(get_mag_strength_tesla(25, 165) * 1e9, 36310, 145 + 363); + EXPECT_NEAR(get_mag_strength_tesla(25, 170) * 1e9, 35826, 145 + 358); EXPECT_NEAR(get_mag_strength_tesla(25, 175) * 1e9, 35517, 145 + 355); EXPECT_NEAR(get_mag_strength_tesla(25, 180) * 1e9, 35366, 145 + 354); EXPECT_NEAR(get_mag_strength_tesla(30, -180) * 1e9, 37220, 145 + 372); - EXPECT_NEAR(get_mag_strength_tesla(30, -175) * 1e9, 37176, 145 + 372); - EXPECT_NEAR(get_mag_strength_tesla(30, -170) * 1e9, 37265, 145 + 373); - EXPECT_NEAR(get_mag_strength_tesla(30, -165) * 1e9, 37471, 145 + 375); - EXPECT_NEAR(get_mag_strength_tesla(30, -160) * 1e9, 37788, 145 + 378); - EXPECT_NEAR(get_mag_strength_tesla(30, -155) * 1e9, 38219, 145 + 382); - EXPECT_NEAR(get_mag_strength_tesla(30, -150) * 1e9, 38765, 145 + 388); - EXPECT_NEAR(get_mag_strength_tesla(30, -145) * 1e9, 39423, 145 + 394); - EXPECT_NEAR(get_mag_strength_tesla(30, -140) * 1e9, 40178, 145 + 402); - EXPECT_NEAR(get_mag_strength_tesla(30, -135) * 1e9, 41006, 145 + 410); - EXPECT_NEAR(get_mag_strength_tesla(30, -130) * 1e9, 41875, 145 + 419); - EXPECT_NEAR(get_mag_strength_tesla(30, -125) * 1e9, 42756, 145 + 428); - EXPECT_NEAR(get_mag_strength_tesla(30, -120) * 1e9, 43617, 145 + 436); - EXPECT_NEAR(get_mag_strength_tesla(30, -115) * 1e9, 44431, 145 + 444); - EXPECT_NEAR(get_mag_strength_tesla(30, -110) * 1e9, 45166, 145 + 452); - EXPECT_NEAR(get_mag_strength_tesla(30, -105) * 1e9, 45783, 145 + 458); - EXPECT_NEAR(get_mag_strength_tesla(30, -100) * 1e9, 46237, 145 + 462); - EXPECT_NEAR(get_mag_strength_tesla(30, -95) * 1e9, 46486, 145 + 465); - EXPECT_NEAR(get_mag_strength_tesla(30, -90) * 1e9, 46497, 145 + 465); - EXPECT_NEAR(get_mag_strength_tesla(30, -85) * 1e9, 46254, 145 + 463); - EXPECT_NEAR(get_mag_strength_tesla(30, -80) * 1e9, 45768, 145 + 458); - EXPECT_NEAR(get_mag_strength_tesla(30, -75) * 1e9, 45072, 145 + 451); - EXPECT_NEAR(get_mag_strength_tesla(30, -70) * 1e9, 44223, 145 + 442); - EXPECT_NEAR(get_mag_strength_tesla(30, -65) * 1e9, 43292, 145 + 433); - EXPECT_NEAR(get_mag_strength_tesla(30, -60) * 1e9, 42356, 145 + 424); - EXPECT_NEAR(get_mag_strength_tesla(30, -55) * 1e9, 41489, 145 + 415); - EXPECT_NEAR(get_mag_strength_tesla(30, -50) * 1e9, 40745, 145 + 407); - EXPECT_NEAR(get_mag_strength_tesla(30, -45) * 1e9, 40155, 145 + 402); - EXPECT_NEAR(get_mag_strength_tesla(30, -40) * 1e9, 39724, 145 + 397); - EXPECT_NEAR(get_mag_strength_tesla(30, -35) * 1e9, 39442, 145 + 394); - EXPECT_NEAR(get_mag_strength_tesla(30, -30) * 1e9, 39291, 145 + 393); + EXPECT_NEAR(get_mag_strength_tesla(30, -175) * 1e9, 37174, 145 + 372); + EXPECT_NEAR(get_mag_strength_tesla(30, -170) * 1e9, 37262, 145 + 373); + EXPECT_NEAR(get_mag_strength_tesla(30, -165) * 1e9, 37466, 145 + 375); + EXPECT_NEAR(get_mag_strength_tesla(30, -160) * 1e9, 37783, 145 + 378); + EXPECT_NEAR(get_mag_strength_tesla(30, -155) * 1e9, 38212, 145 + 382); + EXPECT_NEAR(get_mag_strength_tesla(30, -150) * 1e9, 38758, 145 + 388); + EXPECT_NEAR(get_mag_strength_tesla(30, -145) * 1e9, 39414, 145 + 394); + EXPECT_NEAR(get_mag_strength_tesla(30, -140) * 1e9, 40169, 145 + 402); + EXPECT_NEAR(get_mag_strength_tesla(30, -135) * 1e9, 40996, 145 + 410); + EXPECT_NEAR(get_mag_strength_tesla(30, -130) * 1e9, 41865, 145 + 419); + EXPECT_NEAR(get_mag_strength_tesla(30, -125) * 1e9, 42744, 145 + 427); + EXPECT_NEAR(get_mag_strength_tesla(30, -120) * 1e9, 43605, 145 + 436); + EXPECT_NEAR(get_mag_strength_tesla(30, -115) * 1e9, 44419, 145 + 444); + EXPECT_NEAR(get_mag_strength_tesla(30, -110) * 1e9, 45153, 145 + 452); + EXPECT_NEAR(get_mag_strength_tesla(30, -105) * 1e9, 45769, 145 + 458); + EXPECT_NEAR(get_mag_strength_tesla(30, -100) * 1e9, 46222, 145 + 462); + EXPECT_NEAR(get_mag_strength_tesla(30, -95) * 1e9, 46471, 145 + 465); + EXPECT_NEAR(get_mag_strength_tesla(30, -90) * 1e9, 46481, 145 + 465); + EXPECT_NEAR(get_mag_strength_tesla(30, -85) * 1e9, 46238, 145 + 462); + EXPECT_NEAR(get_mag_strength_tesla(30, -80) * 1e9, 45752, 145 + 458); + EXPECT_NEAR(get_mag_strength_tesla(30, -75) * 1e9, 45057, 145 + 451); + EXPECT_NEAR(get_mag_strength_tesla(30, -70) * 1e9, 44208, 145 + 442); + EXPECT_NEAR(get_mag_strength_tesla(30, -65) * 1e9, 43278, 145 + 433); + EXPECT_NEAR(get_mag_strength_tesla(30, -60) * 1e9, 42344, 145 + 423); + EXPECT_NEAR(get_mag_strength_tesla(30, -55) * 1e9, 41479, 145 + 415); + EXPECT_NEAR(get_mag_strength_tesla(30, -50) * 1e9, 40736, 145 + 407); + EXPECT_NEAR(get_mag_strength_tesla(30, -45) * 1e9, 40148, 145 + 401); + EXPECT_NEAR(get_mag_strength_tesla(30, -40) * 1e9, 39720, 145 + 397); + EXPECT_NEAR(get_mag_strength_tesla(30, -35) * 1e9, 39439, 145 + 394); + EXPECT_NEAR(get_mag_strength_tesla(30, -30) * 1e9, 39289, 145 + 393); EXPECT_NEAR(get_mag_strength_tesla(30, -25) * 1e9, 39260, 145 + 393); - EXPECT_NEAR(get_mag_strength_tesla(30, -20) * 1e9, 39348, 145 + 393); - EXPECT_NEAR(get_mag_strength_tesla(30, -15) * 1e9, 39556, 145 + 396); - EXPECT_NEAR(get_mag_strength_tesla(30, -10) * 1e9, 39878, 145 + 399); - EXPECT_NEAR(get_mag_strength_tesla(30, -5) * 1e9, 40292, 145 + 403); - EXPECT_NEAR(get_mag_strength_tesla(30, 0) * 1e9, 40764, 145 + 408); - EXPECT_NEAR(get_mag_strength_tesla(30, 5) * 1e9, 41259, 145 + 413); - EXPECT_NEAR(get_mag_strength_tesla(30, 10) * 1e9, 41755, 145 + 418); - EXPECT_NEAR(get_mag_strength_tesla(30, 15) * 1e9, 42245, 145 + 422); - EXPECT_NEAR(get_mag_strength_tesla(30, 20) * 1e9, 42741, 145 + 427); - EXPECT_NEAR(get_mag_strength_tesla(30, 25) * 1e9, 43259, 145 + 433); - EXPECT_NEAR(get_mag_strength_tesla(30, 30) * 1e9, 43809, 145 + 438); - EXPECT_NEAR(get_mag_strength_tesla(30, 35) * 1e9, 44387, 145 + 444); - EXPECT_NEAR(get_mag_strength_tesla(30, 40) * 1e9, 44979, 145 + 450); - EXPECT_NEAR(get_mag_strength_tesla(30, 45) * 1e9, 45570, 145 + 456); - EXPECT_NEAR(get_mag_strength_tesla(30, 50) * 1e9, 46159, 145 + 462); - EXPECT_NEAR(get_mag_strength_tesla(30, 55) * 1e9, 46752, 145 + 468); - EXPECT_NEAR(get_mag_strength_tesla(30, 60) * 1e9, 47363, 145 + 474); - EXPECT_NEAR(get_mag_strength_tesla(30, 65) * 1e9, 47998, 145 + 480); - EXPECT_NEAR(get_mag_strength_tesla(30, 70) * 1e9, 48646, 145 + 486); - EXPECT_NEAR(get_mag_strength_tesla(30, 75) * 1e9, 49281, 145 + 493); - EXPECT_NEAR(get_mag_strength_tesla(30, 80) * 1e9, 49864, 145 + 499); - EXPECT_NEAR(get_mag_strength_tesla(30, 85) * 1e9, 50357, 145 + 504); - EXPECT_NEAR(get_mag_strength_tesla(30, 90) * 1e9, 50725, 145 + 507); - EXPECT_NEAR(get_mag_strength_tesla(30, 95) * 1e9, 50938, 145 + 509); - EXPECT_NEAR(get_mag_strength_tesla(30, 100) * 1e9, 50968, 145 + 510); - EXPECT_NEAR(get_mag_strength_tesla(30, 105) * 1e9, 50787, 145 + 508); - EXPECT_NEAR(get_mag_strength_tesla(30, 110) * 1e9, 50371, 145 + 504); - EXPECT_NEAR(get_mag_strength_tesla(30, 115) * 1e9, 49703, 145 + 497); - EXPECT_NEAR(get_mag_strength_tesla(30, 120) * 1e9, 48790, 145 + 488); - EXPECT_NEAR(get_mag_strength_tesla(30, 125) * 1e9, 47664, 145 + 477); - EXPECT_NEAR(get_mag_strength_tesla(30, 130) * 1e9, 46381, 145 + 464); - EXPECT_NEAR(get_mag_strength_tesla(30, 135) * 1e9, 45012, 145 + 450); - EXPECT_NEAR(get_mag_strength_tesla(30, 140) * 1e9, 43632, 145 + 436); - EXPECT_NEAR(get_mag_strength_tesla(30, 145) * 1e9, 42306, 145 + 423); - EXPECT_NEAR(get_mag_strength_tesla(30, 150) * 1e9, 41084, 145 + 411); - EXPECT_NEAR(get_mag_strength_tesla(30, 155) * 1e9, 40002, 145 + 400); - EXPECT_NEAR(get_mag_strength_tesla(30, 160) * 1e9, 39083, 145 + 391); - EXPECT_NEAR(get_mag_strength_tesla(30, 165) * 1e9, 38343, 145 + 383); - EXPECT_NEAR(get_mag_strength_tesla(30, 170) * 1e9, 37788, 145 + 378); - EXPECT_NEAR(get_mag_strength_tesla(30, 175) * 1e9, 37418, 145 + 374); + EXPECT_NEAR(get_mag_strength_tesla(30, -20) * 1e9, 39349, 145 + 393); + EXPECT_NEAR(get_mag_strength_tesla(30, -15) * 1e9, 39558, 145 + 396); + EXPECT_NEAR(get_mag_strength_tesla(30, -10) * 1e9, 39881, 145 + 399); + EXPECT_NEAR(get_mag_strength_tesla(30, -5) * 1e9, 40295, 145 + 403); + EXPECT_NEAR(get_mag_strength_tesla(30, 0) * 1e9, 40768, 145 + 408); + EXPECT_NEAR(get_mag_strength_tesla(30, 5) * 1e9, 41265, 145 + 413); + EXPECT_NEAR(get_mag_strength_tesla(30, 10) * 1e9, 41761, 145 + 418); + EXPECT_NEAR(get_mag_strength_tesla(30, 15) * 1e9, 42251, 145 + 423); + EXPECT_NEAR(get_mag_strength_tesla(30, 20) * 1e9, 42748, 145 + 427); + EXPECT_NEAR(get_mag_strength_tesla(30, 25) * 1e9, 43267, 145 + 433); + EXPECT_NEAR(get_mag_strength_tesla(30, 30) * 1e9, 43817, 145 + 438); + EXPECT_NEAR(get_mag_strength_tesla(30, 35) * 1e9, 44396, 145 + 444); + EXPECT_NEAR(get_mag_strength_tesla(30, 40) * 1e9, 44988, 145 + 450); + EXPECT_NEAR(get_mag_strength_tesla(30, 45) * 1e9, 45579, 145 + 456); + EXPECT_NEAR(get_mag_strength_tesla(30, 50) * 1e9, 46168, 145 + 462); + EXPECT_NEAR(get_mag_strength_tesla(30, 55) * 1e9, 46761, 145 + 468); + EXPECT_NEAR(get_mag_strength_tesla(30, 60) * 1e9, 47373, 145 + 474); + EXPECT_NEAR(get_mag_strength_tesla(30, 65) * 1e9, 48008, 145 + 480); + EXPECT_NEAR(get_mag_strength_tesla(30, 70) * 1e9, 48658, 145 + 487); + EXPECT_NEAR(get_mag_strength_tesla(30, 75) * 1e9, 49293, 145 + 493); + EXPECT_NEAR(get_mag_strength_tesla(30, 80) * 1e9, 49877, 145 + 499); + EXPECT_NEAR(get_mag_strength_tesla(30, 85) * 1e9, 50370, 145 + 504); + EXPECT_NEAR(get_mag_strength_tesla(30, 90) * 1e9, 50737, 145 + 507); + EXPECT_NEAR(get_mag_strength_tesla(30, 95) * 1e9, 50950, 145 + 509); + EXPECT_NEAR(get_mag_strength_tesla(30, 100) * 1e9, 50979, 145 + 510); + EXPECT_NEAR(get_mag_strength_tesla(30, 105) * 1e9, 50797, 145 + 508); + EXPECT_NEAR(get_mag_strength_tesla(30, 110) * 1e9, 50379, 145 + 504); + EXPECT_NEAR(get_mag_strength_tesla(30, 115) * 1e9, 49710, 145 + 497); + EXPECT_NEAR(get_mag_strength_tesla(30, 120) * 1e9, 48797, 145 + 488); + EXPECT_NEAR(get_mag_strength_tesla(30, 125) * 1e9, 47670, 145 + 477); + EXPECT_NEAR(get_mag_strength_tesla(30, 130) * 1e9, 46386, 145 + 464); + EXPECT_NEAR(get_mag_strength_tesla(30, 135) * 1e9, 45016, 145 + 450); + EXPECT_NEAR(get_mag_strength_tesla(30, 140) * 1e9, 43636, 145 + 436); + EXPECT_NEAR(get_mag_strength_tesla(30, 145) * 1e9, 42309, 145 + 423); + EXPECT_NEAR(get_mag_strength_tesla(30, 150) * 1e9, 41087, 145 + 411); + EXPECT_NEAR(get_mag_strength_tesla(30, 155) * 1e9, 40005, 145 + 400); + EXPECT_NEAR(get_mag_strength_tesla(30, 160) * 1e9, 39086, 145 + 391); + EXPECT_NEAR(get_mag_strength_tesla(30, 165) * 1e9, 38345, 145 + 383); + EXPECT_NEAR(get_mag_strength_tesla(30, 170) * 1e9, 37790, 145 + 378); + EXPECT_NEAR(get_mag_strength_tesla(30, 175) * 1e9, 37419, 145 + 374); EXPECT_NEAR(get_mag_strength_tesla(30, 180) * 1e9, 37220, 145 + 372); - EXPECT_NEAR(get_mag_strength_tesla(35, -180) * 1e9, 39522, 145 + 395); - EXPECT_NEAR(get_mag_strength_tesla(35, -175) * 1e9, 39441, 145 + 394); - EXPECT_NEAR(get_mag_strength_tesla(35, -170) * 1e9, 39520, 145 + 395); - EXPECT_NEAR(get_mag_strength_tesla(35, -165) * 1e9, 39748, 145 + 397); - EXPECT_NEAR(get_mag_strength_tesla(35, -160) * 1e9, 40116, 145 + 401); - EXPECT_NEAR(get_mag_strength_tesla(35, -155) * 1e9, 40619, 145 + 406); - EXPECT_NEAR(get_mag_strength_tesla(35, -150) * 1e9, 41249, 145 + 412); - EXPECT_NEAR(get_mag_strength_tesla(35, -145) * 1e9, 41991, 145 + 420); - EXPECT_NEAR(get_mag_strength_tesla(35, -140) * 1e9, 42826, 145 + 428); - EXPECT_NEAR(get_mag_strength_tesla(35, -135) * 1e9, 43723, 145 + 437); - EXPECT_NEAR(get_mag_strength_tesla(35, -130) * 1e9, 44653, 145 + 447); - EXPECT_NEAR(get_mag_strength_tesla(35, -125) * 1e9, 45584, 145 + 456); - EXPECT_NEAR(get_mag_strength_tesla(35, -120) * 1e9, 46487, 145 + 465); - EXPECT_NEAR(get_mag_strength_tesla(35, -115) * 1e9, 47334, 145 + 473); - EXPECT_NEAR(get_mag_strength_tesla(35, -110) * 1e9, 48092, 145 + 481); - EXPECT_NEAR(get_mag_strength_tesla(35, -105) * 1e9, 48725, 145 + 487); - EXPECT_NEAR(get_mag_strength_tesla(35, -100) * 1e9, 49192, 145 + 492); - EXPECT_NEAR(get_mag_strength_tesla(35, -95) * 1e9, 49452, 145 + 495); - EXPECT_NEAR(get_mag_strength_tesla(35, -90) * 1e9, 49474, 145 + 495); - EXPECT_NEAR(get_mag_strength_tesla(35, -85) * 1e9, 49245, 145 + 492); - EXPECT_NEAR(get_mag_strength_tesla(35, -80) * 1e9, 48772, 145 + 488); - EXPECT_NEAR(get_mag_strength_tesla(35, -75) * 1e9, 48086, 145 + 481); - EXPECT_NEAR(get_mag_strength_tesla(35, -70) * 1e9, 47241, 145 + 472); - EXPECT_NEAR(get_mag_strength_tesla(35, -65) * 1e9, 46307, 145 + 463); - EXPECT_NEAR(get_mag_strength_tesla(35, -60) * 1e9, 45357, 145 + 454); - EXPECT_NEAR(get_mag_strength_tesla(35, -55) * 1e9, 44462, 145 + 445); - EXPECT_NEAR(get_mag_strength_tesla(35, -50) * 1e9, 43676, 145 + 437); - EXPECT_NEAR(get_mag_strength_tesla(35, -45) * 1e9, 43029, 145 + 430); - EXPECT_NEAR(get_mag_strength_tesla(35, -40) * 1e9, 42531, 145 + 425); - EXPECT_NEAR(get_mag_strength_tesla(35, -35) * 1e9, 42174, 145 + 422); - EXPECT_NEAR(get_mag_strength_tesla(35, -30) * 1e9, 41947, 145 + 419); - EXPECT_NEAR(get_mag_strength_tesla(35, -25) * 1e9, 41843, 145 + 418); - EXPECT_NEAR(get_mag_strength_tesla(35, -20) * 1e9, 41861, 145 + 419); - EXPECT_NEAR(get_mag_strength_tesla(35, -15) * 1e9, 42002, 145 + 420); - EXPECT_NEAR(get_mag_strength_tesla(35, -10) * 1e9, 42259, 145 + 423); - EXPECT_NEAR(get_mag_strength_tesla(35, -5) * 1e9, 42614, 145 + 426); - EXPECT_NEAR(get_mag_strength_tesla(35, 0) * 1e9, 43035, 145 + 430); - EXPECT_NEAR(get_mag_strength_tesla(35, 5) * 1e9, 43490, 145 + 435); - EXPECT_NEAR(get_mag_strength_tesla(35, 10) * 1e9, 43957, 145 + 440); - EXPECT_NEAR(get_mag_strength_tesla(35, 15) * 1e9, 44426, 145 + 444); - EXPECT_NEAR(get_mag_strength_tesla(35, 20) * 1e9, 44904, 145 + 449); - EXPECT_NEAR(get_mag_strength_tesla(35, 25) * 1e9, 45404, 145 + 454); - EXPECT_NEAR(get_mag_strength_tesla(35, 30) * 1e9, 45936, 145 + 459); - EXPECT_NEAR(get_mag_strength_tesla(35, 35) * 1e9, 46501, 145 + 465); - EXPECT_NEAR(get_mag_strength_tesla(35, 40) * 1e9, 47093, 145 + 471); - EXPECT_NEAR(get_mag_strength_tesla(35, 45) * 1e9, 47706, 145 + 477); - EXPECT_NEAR(get_mag_strength_tesla(35, 50) * 1e9, 48340, 145 + 483); - EXPECT_NEAR(get_mag_strength_tesla(35, 55) * 1e9, 48999, 145 + 490); - EXPECT_NEAR(get_mag_strength_tesla(35, 60) * 1e9, 49689, 145 + 497); - EXPECT_NEAR(get_mag_strength_tesla(35, 65) * 1e9, 50408, 145 + 504); - EXPECT_NEAR(get_mag_strength_tesla(35, 70) * 1e9, 51140, 145 + 511); - EXPECT_NEAR(get_mag_strength_tesla(35, 75) * 1e9, 51855, 145 + 519); - EXPECT_NEAR(get_mag_strength_tesla(35, 80) * 1e9, 52514, 145 + 525); - EXPECT_NEAR(get_mag_strength_tesla(35, 85) * 1e9, 53075, 145 + 531); - EXPECT_NEAR(get_mag_strength_tesla(35, 90) * 1e9, 53502, 145 + 535); - EXPECT_NEAR(get_mag_strength_tesla(35, 95) * 1e9, 53759, 145 + 538); - EXPECT_NEAR(get_mag_strength_tesla(35, 100) * 1e9, 53817, 145 + 538); - EXPECT_NEAR(get_mag_strength_tesla(35, 105) * 1e9, 53648, 145 + 536); - EXPECT_NEAR(get_mag_strength_tesla(35, 110) * 1e9, 53230, 145 + 532); - EXPECT_NEAR(get_mag_strength_tesla(35, 115) * 1e9, 52552, 145 + 526); - EXPECT_NEAR(get_mag_strength_tesla(35, 120) * 1e9, 51624, 145 + 516); - EXPECT_NEAR(get_mag_strength_tesla(35, 125) * 1e9, 50479, 145 + 505); - EXPECT_NEAR(get_mag_strength_tesla(35, 130) * 1e9, 49172, 145 + 492); - EXPECT_NEAR(get_mag_strength_tesla(35, 135) * 1e9, 47775, 145 + 478); - EXPECT_NEAR(get_mag_strength_tesla(35, 140) * 1e9, 46361, 145 + 464); - EXPECT_NEAR(get_mag_strength_tesla(35, 145) * 1e9, 44994, 145 + 450); - EXPECT_NEAR(get_mag_strength_tesla(35, 150) * 1e9, 43727, 145 + 437); - EXPECT_NEAR(get_mag_strength_tesla(35, 155) * 1e9, 42595, 145 + 426); - EXPECT_NEAR(get_mag_strength_tesla(35, 160) * 1e9, 41622, 145 + 416); - EXPECT_NEAR(get_mag_strength_tesla(35, 165) * 1e9, 40823, 145 + 408); - EXPECT_NEAR(get_mag_strength_tesla(35, 170) * 1e9, 40206, 145 + 402); - EXPECT_NEAR(get_mag_strength_tesla(35, 175) * 1e9, 39773, 145 + 398); - EXPECT_NEAR(get_mag_strength_tesla(35, 180) * 1e9, 39522, 145 + 395); - EXPECT_NEAR(get_mag_strength_tesla(40, -180) * 1e9, 42219, 145 + 422); - EXPECT_NEAR(get_mag_strength_tesla(40, -175) * 1e9, 42106, 145 + 421); - EXPECT_NEAR(get_mag_strength_tesla(40, -170) * 1e9, 42168, 145 + 422); - EXPECT_NEAR(get_mag_strength_tesla(40, -165) * 1e9, 42399, 145 + 424); - EXPECT_NEAR(get_mag_strength_tesla(40, -160) * 1e9, 42790, 145 + 428); - EXPECT_NEAR(get_mag_strength_tesla(40, -155) * 1e9, 43330, 145 + 433); - EXPECT_NEAR(get_mag_strength_tesla(40, -150) * 1e9, 44002, 145 + 440); - EXPECT_NEAR(get_mag_strength_tesla(40, -145) * 1e9, 44785, 145 + 448); - EXPECT_NEAR(get_mag_strength_tesla(40, -140) * 1e9, 45652, 145 + 457); - EXPECT_NEAR(get_mag_strength_tesla(40, -135) * 1e9, 46571, 145 + 466); - EXPECT_NEAR(get_mag_strength_tesla(40, -130) * 1e9, 47511, 145 + 475); - EXPECT_NEAR(get_mag_strength_tesla(40, -125) * 1e9, 48441, 145 + 484); - EXPECT_NEAR(get_mag_strength_tesla(40, -120) * 1e9, 49334, 145 + 493); - EXPECT_NEAR(get_mag_strength_tesla(40, -115) * 1e9, 50161, 145 + 502); - EXPECT_NEAR(get_mag_strength_tesla(40, -110) * 1e9, 50893, 145 + 509); - EXPECT_NEAR(get_mag_strength_tesla(40, -105) * 1e9, 51498, 145 + 515); - EXPECT_NEAR(get_mag_strength_tesla(40, -100) * 1e9, 51938, 145 + 519); - EXPECT_NEAR(get_mag_strength_tesla(40, -95) * 1e9, 52180, 145 + 522); - EXPECT_NEAR(get_mag_strength_tesla(40, -90) * 1e9, 52196, 145 + 522); - EXPECT_NEAR(get_mag_strength_tesla(40, -85) * 1e9, 51975, 145 + 520); - EXPECT_NEAR(get_mag_strength_tesla(40, -80) * 1e9, 51521, 145 + 515); - EXPECT_NEAR(get_mag_strength_tesla(40, -75) * 1e9, 50864, 145 + 509); - EXPECT_NEAR(get_mag_strength_tesla(40, -70) * 1e9, 50050, 145 + 501); - EXPECT_NEAR(get_mag_strength_tesla(40, -65) * 1e9, 49142, 145 + 491); - EXPECT_NEAR(get_mag_strength_tesla(40, -60) * 1e9, 48206, 145 + 482); - EXPECT_NEAR(get_mag_strength_tesla(40, -55) * 1e9, 47307, 145 + 473); - EXPECT_NEAR(get_mag_strength_tesla(40, -50) * 1e9, 46495, 145 + 465); - EXPECT_NEAR(get_mag_strength_tesla(40, -45) * 1e9, 45801, 145 + 458); - EXPECT_NEAR(get_mag_strength_tesla(40, -40) * 1e9, 45240, 145 + 452); - EXPECT_NEAR(get_mag_strength_tesla(40, -35) * 1e9, 44811, 145 + 448); - EXPECT_NEAR(get_mag_strength_tesla(40, -30) * 1e9, 44509, 145 + 445); + EXPECT_NEAR(get_mag_strength_tesla(35, -180) * 1e9, 39521, 145 + 395); + EXPECT_NEAR(get_mag_strength_tesla(35, -175) * 1e9, 39439, 145 + 394); + EXPECT_NEAR(get_mag_strength_tesla(35, -170) * 1e9, 39517, 145 + 395); + EXPECT_NEAR(get_mag_strength_tesla(35, -165) * 1e9, 39743, 145 + 397); + EXPECT_NEAR(get_mag_strength_tesla(35, -160) * 1e9, 40110, 145 + 401); + EXPECT_NEAR(get_mag_strength_tesla(35, -155) * 1e9, 40612, 145 + 406); + EXPECT_NEAR(get_mag_strength_tesla(35, -150) * 1e9, 41240, 145 + 412); + EXPECT_NEAR(get_mag_strength_tesla(35, -145) * 1e9, 41982, 145 + 420); + EXPECT_NEAR(get_mag_strength_tesla(35, -140) * 1e9, 42816, 145 + 428); + EXPECT_NEAR(get_mag_strength_tesla(35, -135) * 1e9, 43712, 145 + 437); + EXPECT_NEAR(get_mag_strength_tesla(35, -130) * 1e9, 44641, 145 + 446); + EXPECT_NEAR(get_mag_strength_tesla(35, -125) * 1e9, 45572, 145 + 456); + EXPECT_NEAR(get_mag_strength_tesla(35, -120) * 1e9, 46474, 145 + 465); + EXPECT_NEAR(get_mag_strength_tesla(35, -115) * 1e9, 47320, 145 + 473); + EXPECT_NEAR(get_mag_strength_tesla(35, -110) * 1e9, 48078, 145 + 481); + EXPECT_NEAR(get_mag_strength_tesla(35, -105) * 1e9, 48711, 145 + 487); + EXPECT_NEAR(get_mag_strength_tesla(35, -100) * 1e9, 49177, 145 + 492); + EXPECT_NEAR(get_mag_strength_tesla(35, -95) * 1e9, 49437, 145 + 494); + EXPECT_NEAR(get_mag_strength_tesla(35, -90) * 1e9, 49459, 145 + 495); + EXPECT_NEAR(get_mag_strength_tesla(35, -85) * 1e9, 49230, 145 + 492); + EXPECT_NEAR(get_mag_strength_tesla(35, -80) * 1e9, 48756, 145 + 488); + EXPECT_NEAR(get_mag_strength_tesla(35, -75) * 1e9, 48071, 145 + 481); + EXPECT_NEAR(get_mag_strength_tesla(35, -70) * 1e9, 47227, 145 + 472); + EXPECT_NEAR(get_mag_strength_tesla(35, -65) * 1e9, 46294, 145 + 463); + EXPECT_NEAR(get_mag_strength_tesla(35, -60) * 1e9, 45346, 145 + 453); + EXPECT_NEAR(get_mag_strength_tesla(35, -55) * 1e9, 44453, 145 + 445); + EXPECT_NEAR(get_mag_strength_tesla(35, -50) * 1e9, 43668, 145 + 437); + EXPECT_NEAR(get_mag_strength_tesla(35, -45) * 1e9, 43023, 145 + 430); + EXPECT_NEAR(get_mag_strength_tesla(35, -40) * 1e9, 42527, 145 + 425); + EXPECT_NEAR(get_mag_strength_tesla(35, -35) * 1e9, 42171, 145 + 422); + EXPECT_NEAR(get_mag_strength_tesla(35, -30) * 1e9, 41946, 145 + 419); + EXPECT_NEAR(get_mag_strength_tesla(35, -25) * 1e9, 41842, 145 + 418); + EXPECT_NEAR(get_mag_strength_tesla(35, -20) * 1e9, 41862, 145 + 419); + EXPECT_NEAR(get_mag_strength_tesla(35, -15) * 1e9, 42004, 145 + 420); + EXPECT_NEAR(get_mag_strength_tesla(35, -10) * 1e9, 42262, 145 + 423); + EXPECT_NEAR(get_mag_strength_tesla(35, -5) * 1e9, 42618, 145 + 426); + EXPECT_NEAR(get_mag_strength_tesla(35, 0) * 1e9, 43040, 145 + 430); + EXPECT_NEAR(get_mag_strength_tesla(35, 5) * 1e9, 43496, 145 + 435); + EXPECT_NEAR(get_mag_strength_tesla(35, 10) * 1e9, 43963, 145 + 440); + EXPECT_NEAR(get_mag_strength_tesla(35, 15) * 1e9, 44433, 145 + 444); + EXPECT_NEAR(get_mag_strength_tesla(35, 20) * 1e9, 44912, 145 + 449); + EXPECT_NEAR(get_mag_strength_tesla(35, 25) * 1e9, 45412, 145 + 454); + EXPECT_NEAR(get_mag_strength_tesla(35, 30) * 1e9, 45945, 145 + 459); + EXPECT_NEAR(get_mag_strength_tesla(35, 35) * 1e9, 46510, 145 + 465); + EXPECT_NEAR(get_mag_strength_tesla(35, 40) * 1e9, 47103, 145 + 471); + EXPECT_NEAR(get_mag_strength_tesla(35, 45) * 1e9, 47716, 145 + 477); + EXPECT_NEAR(get_mag_strength_tesla(35, 50) * 1e9, 48350, 145 + 483); + EXPECT_NEAR(get_mag_strength_tesla(35, 55) * 1e9, 49009, 145 + 490); + EXPECT_NEAR(get_mag_strength_tesla(35, 60) * 1e9, 49699, 145 + 497); + EXPECT_NEAR(get_mag_strength_tesla(35, 65) * 1e9, 50419, 145 + 504); + EXPECT_NEAR(get_mag_strength_tesla(35, 70) * 1e9, 51152, 145 + 512); + EXPECT_NEAR(get_mag_strength_tesla(35, 75) * 1e9, 51868, 145 + 519); + EXPECT_NEAR(get_mag_strength_tesla(35, 80) * 1e9, 52527, 145 + 525); + EXPECT_NEAR(get_mag_strength_tesla(35, 85) * 1e9, 53088, 145 + 531); + EXPECT_NEAR(get_mag_strength_tesla(35, 90) * 1e9, 53514, 145 + 535); + EXPECT_NEAR(get_mag_strength_tesla(35, 95) * 1e9, 53771, 145 + 538); + EXPECT_NEAR(get_mag_strength_tesla(35, 100) * 1e9, 53828, 145 + 538); + EXPECT_NEAR(get_mag_strength_tesla(35, 105) * 1e9, 53658, 145 + 537); + EXPECT_NEAR(get_mag_strength_tesla(35, 110) * 1e9, 53238, 145 + 532); + EXPECT_NEAR(get_mag_strength_tesla(35, 115) * 1e9, 52560, 145 + 526); + EXPECT_NEAR(get_mag_strength_tesla(35, 120) * 1e9, 51630, 145 + 516); + EXPECT_NEAR(get_mag_strength_tesla(35, 125) * 1e9, 50484, 145 + 505); + EXPECT_NEAR(get_mag_strength_tesla(35, 130) * 1e9, 49177, 145 + 492); + EXPECT_NEAR(get_mag_strength_tesla(35, 135) * 1e9, 47780, 145 + 478); + EXPECT_NEAR(get_mag_strength_tesla(35, 140) * 1e9, 46365, 145 + 464); + EXPECT_NEAR(get_mag_strength_tesla(35, 145) * 1e9, 44998, 145 + 450); + EXPECT_NEAR(get_mag_strength_tesla(35, 150) * 1e9, 43731, 145 + 437); + EXPECT_NEAR(get_mag_strength_tesla(35, 155) * 1e9, 42599, 145 + 426); + EXPECT_NEAR(get_mag_strength_tesla(35, 160) * 1e9, 41625, 145 + 416); + EXPECT_NEAR(get_mag_strength_tesla(35, 165) * 1e9, 40826, 145 + 408); + EXPECT_NEAR(get_mag_strength_tesla(35, 170) * 1e9, 40208, 145 + 402); + EXPECT_NEAR(get_mag_strength_tesla(35, 175) * 1e9, 39774, 145 + 398); + EXPECT_NEAR(get_mag_strength_tesla(35, 180) * 1e9, 39521, 145 + 395); + EXPECT_NEAR(get_mag_strength_tesla(40, -180) * 1e9, 42218, 145 + 422); + EXPECT_NEAR(get_mag_strength_tesla(40, -175) * 1e9, 42104, 145 + 421); + EXPECT_NEAR(get_mag_strength_tesla(40, -170) * 1e9, 42164, 145 + 422); + EXPECT_NEAR(get_mag_strength_tesla(40, -165) * 1e9, 42394, 145 + 424); + EXPECT_NEAR(get_mag_strength_tesla(40, -160) * 1e9, 42783, 145 + 428); + EXPECT_NEAR(get_mag_strength_tesla(40, -155) * 1e9, 43322, 145 + 433); + EXPECT_NEAR(get_mag_strength_tesla(40, -150) * 1e9, 43993, 145 + 440); + EXPECT_NEAR(get_mag_strength_tesla(40, -145) * 1e9, 44775, 145 + 448); + EXPECT_NEAR(get_mag_strength_tesla(40, -140) * 1e9, 45641, 145 + 456); + EXPECT_NEAR(get_mag_strength_tesla(40, -135) * 1e9, 46559, 145 + 466); + EXPECT_NEAR(get_mag_strength_tesla(40, -130) * 1e9, 47498, 145 + 475); + EXPECT_NEAR(get_mag_strength_tesla(40, -125) * 1e9, 48428, 145 + 484); + EXPECT_NEAR(get_mag_strength_tesla(40, -120) * 1e9, 49320, 145 + 493); + EXPECT_NEAR(get_mag_strength_tesla(40, -115) * 1e9, 50147, 145 + 501); + EXPECT_NEAR(get_mag_strength_tesla(40, -110) * 1e9, 50879, 145 + 509); + EXPECT_NEAR(get_mag_strength_tesla(40, -105) * 1e9, 51483, 145 + 515); + EXPECT_NEAR(get_mag_strength_tesla(40, -100) * 1e9, 51923, 145 + 519); + EXPECT_NEAR(get_mag_strength_tesla(40, -95) * 1e9, 52165, 145 + 522); + EXPECT_NEAR(get_mag_strength_tesla(40, -90) * 1e9, 52182, 145 + 522); + EXPECT_NEAR(get_mag_strength_tesla(40, -85) * 1e9, 51960, 145 + 520); + EXPECT_NEAR(get_mag_strength_tesla(40, -80) * 1e9, 51507, 145 + 515); + EXPECT_NEAR(get_mag_strength_tesla(40, -75) * 1e9, 50851, 145 + 509); + EXPECT_NEAR(get_mag_strength_tesla(40, -70) * 1e9, 50038, 145 + 500); + EXPECT_NEAR(get_mag_strength_tesla(40, -65) * 1e9, 49130, 145 + 491); + EXPECT_NEAR(get_mag_strength_tesla(40, -60) * 1e9, 48196, 145 + 482); + EXPECT_NEAR(get_mag_strength_tesla(40, -55) * 1e9, 47298, 145 + 473); + EXPECT_NEAR(get_mag_strength_tesla(40, -50) * 1e9, 46488, 145 + 465); + EXPECT_NEAR(get_mag_strength_tesla(40, -45) * 1e9, 45796, 145 + 458); + EXPECT_NEAR(get_mag_strength_tesla(40, -40) * 1e9, 45236, 145 + 452); + EXPECT_NEAR(get_mag_strength_tesla(40, -35) * 1e9, 44808, 145 + 448); + EXPECT_NEAR(get_mag_strength_tesla(40, -30) * 1e9, 44508, 145 + 445); EXPECT_NEAR(get_mag_strength_tesla(40, -25) * 1e9, 44330, 145 + 443); - EXPECT_NEAR(get_mag_strength_tesla(40, -20) * 1e9, 44273, 145 + 443); - EXPECT_NEAR(get_mag_strength_tesla(40, -15) * 1e9, 44337, 145 + 443); - EXPECT_NEAR(get_mag_strength_tesla(40, -10) * 1e9, 44516, 145 + 445); - EXPECT_NEAR(get_mag_strength_tesla(40, -5) * 1e9, 44792, 145 + 448); - EXPECT_NEAR(get_mag_strength_tesla(40, 0) * 1e9, 45140, 145 + 451); - EXPECT_NEAR(get_mag_strength_tesla(40, 5) * 1e9, 45531, 145 + 455); - EXPECT_NEAR(get_mag_strength_tesla(40, 10) * 1e9, 45946, 145 + 459); - EXPECT_NEAR(get_mag_strength_tesla(40, 15) * 1e9, 46375, 145 + 464); - EXPECT_NEAR(get_mag_strength_tesla(40, 20) * 1e9, 46821, 145 + 468); - EXPECT_NEAR(get_mag_strength_tesla(40, 25) * 1e9, 47294, 145 + 473); - EXPECT_NEAR(get_mag_strength_tesla(40, 30) * 1e9, 47804, 145 + 478); - EXPECT_NEAR(get_mag_strength_tesla(40, 35) * 1e9, 48358, 145 + 484); - EXPECT_NEAR(get_mag_strength_tesla(40, 40) * 1e9, 48958, 145 + 490); - EXPECT_NEAR(get_mag_strength_tesla(40, 45) * 1e9, 49603, 145 + 496); - EXPECT_NEAR(get_mag_strength_tesla(40, 50) * 1e9, 50293, 145 + 503); - EXPECT_NEAR(get_mag_strength_tesla(40, 55) * 1e9, 51029, 145 + 510); - EXPECT_NEAR(get_mag_strength_tesla(40, 60) * 1e9, 51808, 145 + 518); - EXPECT_NEAR(get_mag_strength_tesla(40, 65) * 1e9, 52618, 145 + 526); - EXPECT_NEAR(get_mag_strength_tesla(40, 70) * 1e9, 53438, 145 + 534); - EXPECT_NEAR(get_mag_strength_tesla(40, 75) * 1e9, 54231, 145 + 542); - EXPECT_NEAR(get_mag_strength_tesla(40, 80) * 1e9, 54959, 145 + 550); - EXPECT_NEAR(get_mag_strength_tesla(40, 85) * 1e9, 55579, 145 + 556); - EXPECT_NEAR(get_mag_strength_tesla(40, 90) * 1e9, 56052, 145 + 561); - EXPECT_NEAR(get_mag_strength_tesla(40, 95) * 1e9, 56342, 145 + 563); - EXPECT_NEAR(get_mag_strength_tesla(40, 100) * 1e9, 56421, 145 + 564); - EXPECT_NEAR(get_mag_strength_tesla(40, 105) * 1e9, 56264, 145 + 563); - EXPECT_NEAR(get_mag_strength_tesla(40, 110) * 1e9, 55854, 145 + 559); - EXPECT_NEAR(get_mag_strength_tesla(40, 115) * 1e9, 55188, 145 + 552); - EXPECT_NEAR(get_mag_strength_tesla(40, 120) * 1e9, 54280, 145 + 543); - EXPECT_NEAR(get_mag_strength_tesla(40, 125) * 1e9, 53164, 145 + 532); - EXPECT_NEAR(get_mag_strength_tesla(40, 130) * 1e9, 51894, 145 + 519); - EXPECT_NEAR(get_mag_strength_tesla(40, 135) * 1e9, 50535, 145 + 505); - EXPECT_NEAR(get_mag_strength_tesla(40, 140) * 1e9, 49154, 145 + 492); - EXPECT_NEAR(get_mag_strength_tesla(40, 145) * 1e9, 47814, 145 + 478); - EXPECT_NEAR(get_mag_strength_tesla(40, 150) * 1e9, 46562, 145 + 466); - EXPECT_NEAR(get_mag_strength_tesla(40, 155) * 1e9, 45434, 145 + 454); - EXPECT_NEAR(get_mag_strength_tesla(40, 160) * 1e9, 44452, 145 + 445); - EXPECT_NEAR(get_mag_strength_tesla(40, 165) * 1e9, 43632, 145 + 436); - EXPECT_NEAR(get_mag_strength_tesla(40, 170) * 1e9, 42983, 145 + 430); - EXPECT_NEAR(get_mag_strength_tesla(40, 175) * 1e9, 42511, 145 + 425); - EXPECT_NEAR(get_mag_strength_tesla(40, 180) * 1e9, 42219, 145 + 422); + EXPECT_NEAR(get_mag_strength_tesla(40, -20) * 1e9, 44274, 145 + 443); + EXPECT_NEAR(get_mag_strength_tesla(40, -15) * 1e9, 44340, 145 + 443); + EXPECT_NEAR(get_mag_strength_tesla(40, -10) * 1e9, 44520, 145 + 445); + EXPECT_NEAR(get_mag_strength_tesla(40, -5) * 1e9, 44797, 145 + 448); + EXPECT_NEAR(get_mag_strength_tesla(40, 0) * 1e9, 45145, 145 + 451); + EXPECT_NEAR(get_mag_strength_tesla(40, 5) * 1e9, 45538, 145 + 455); + EXPECT_NEAR(get_mag_strength_tesla(40, 10) * 1e9, 45954, 145 + 460); + EXPECT_NEAR(get_mag_strength_tesla(40, 15) * 1e9, 46383, 145 + 464); + EXPECT_NEAR(get_mag_strength_tesla(40, 20) * 1e9, 46830, 145 + 468); + EXPECT_NEAR(get_mag_strength_tesla(40, 25) * 1e9, 47303, 145 + 473); + EXPECT_NEAR(get_mag_strength_tesla(40, 30) * 1e9, 47814, 145 + 478); + EXPECT_NEAR(get_mag_strength_tesla(40, 35) * 1e9, 48368, 145 + 484); + EXPECT_NEAR(get_mag_strength_tesla(40, 40) * 1e9, 48968, 145 + 490); + EXPECT_NEAR(get_mag_strength_tesla(40, 45) * 1e9, 49613, 145 + 496); + EXPECT_NEAR(get_mag_strength_tesla(40, 50) * 1e9, 50303, 145 + 503); + EXPECT_NEAR(get_mag_strength_tesla(40, 55) * 1e9, 51039, 145 + 510); + EXPECT_NEAR(get_mag_strength_tesla(40, 60) * 1e9, 51819, 145 + 518); + EXPECT_NEAR(get_mag_strength_tesla(40, 65) * 1e9, 52630, 145 + 526); + EXPECT_NEAR(get_mag_strength_tesla(40, 70) * 1e9, 53450, 145 + 534); + EXPECT_NEAR(get_mag_strength_tesla(40, 75) * 1e9, 54244, 145 + 542); + EXPECT_NEAR(get_mag_strength_tesla(40, 80) * 1e9, 54972, 145 + 550); + EXPECT_NEAR(get_mag_strength_tesla(40, 85) * 1e9, 55592, 145 + 556); + EXPECT_NEAR(get_mag_strength_tesla(40, 90) * 1e9, 56064, 145 + 561); + EXPECT_NEAR(get_mag_strength_tesla(40, 95) * 1e9, 56353, 145 + 564); + EXPECT_NEAR(get_mag_strength_tesla(40, 100) * 1e9, 56431, 145 + 564); + EXPECT_NEAR(get_mag_strength_tesla(40, 105) * 1e9, 56272, 145 + 563); + EXPECT_NEAR(get_mag_strength_tesla(40, 110) * 1e9, 55861, 145 + 559); + EXPECT_NEAR(get_mag_strength_tesla(40, 115) * 1e9, 55194, 145 + 552); + EXPECT_NEAR(get_mag_strength_tesla(40, 120) * 1e9, 54285, 145 + 543); + EXPECT_NEAR(get_mag_strength_tesla(40, 125) * 1e9, 53169, 145 + 532); + EXPECT_NEAR(get_mag_strength_tesla(40, 130) * 1e9, 51898, 145 + 519); + EXPECT_NEAR(get_mag_strength_tesla(40, 135) * 1e9, 50539, 145 + 505); + EXPECT_NEAR(get_mag_strength_tesla(40, 140) * 1e9, 49158, 145 + 492); + EXPECT_NEAR(get_mag_strength_tesla(40, 145) * 1e9, 47818, 145 + 478); + EXPECT_NEAR(get_mag_strength_tesla(40, 150) * 1e9, 46566, 145 + 466); + EXPECT_NEAR(get_mag_strength_tesla(40, 155) * 1e9, 45438, 145 + 454); + EXPECT_NEAR(get_mag_strength_tesla(40, 160) * 1e9, 44456, 145 + 445); + EXPECT_NEAR(get_mag_strength_tesla(40, 165) * 1e9, 43635, 145 + 436); + EXPECT_NEAR(get_mag_strength_tesla(40, 170) * 1e9, 42985, 145 + 430); + EXPECT_NEAR(get_mag_strength_tesla(40, 175) * 1e9, 42512, 145 + 425); + EXPECT_NEAR(get_mag_strength_tesla(40, 180) * 1e9, 42218, 145 + 422); EXPECT_NEAR(get_mag_strength_tesla(45, -180) * 1e9, 45210, 145 + 452); - EXPECT_NEAR(get_mag_strength_tesla(45, -175) * 1e9, 45077, 145 + 451); - EXPECT_NEAR(get_mag_strength_tesla(45, -170) * 1e9, 45121, 145 + 451); - EXPECT_NEAR(get_mag_strength_tesla(45, -165) * 1e9, 45338, 145 + 453); - EXPECT_NEAR(get_mag_strength_tesla(45, -160) * 1e9, 45719, 145 + 457); - EXPECT_NEAR(get_mag_strength_tesla(45, -155) * 1e9, 46251, 145 + 463); - EXPECT_NEAR(get_mag_strength_tesla(45, -150) * 1e9, 46913, 145 + 469); - EXPECT_NEAR(get_mag_strength_tesla(45, -145) * 1e9, 47680, 145 + 477); - EXPECT_NEAR(get_mag_strength_tesla(45, -140) * 1e9, 48521, 145 + 485); - EXPECT_NEAR(get_mag_strength_tesla(45, -135) * 1e9, 49404, 145 + 494); - EXPECT_NEAR(get_mag_strength_tesla(45, -130) * 1e9, 50299, 145 + 503); - EXPECT_NEAR(get_mag_strength_tesla(45, -125) * 1e9, 51175, 145 + 512); - EXPECT_NEAR(get_mag_strength_tesla(45, -120) * 1e9, 52006, 145 + 520); - EXPECT_NEAR(get_mag_strength_tesla(45, -115) * 1e9, 52767, 145 + 528); - EXPECT_NEAR(get_mag_strength_tesla(45, -110) * 1e9, 53430, 145 + 534); - EXPECT_NEAR(get_mag_strength_tesla(45, -105) * 1e9, 53967, 145 + 540); - EXPECT_NEAR(get_mag_strength_tesla(45, -100) * 1e9, 54348, 145 + 543); - EXPECT_NEAR(get_mag_strength_tesla(45, -95) * 1e9, 54547, 145 + 545); - EXPECT_NEAR(get_mag_strength_tesla(45, -90) * 1e9, 54541, 145 + 545); - EXPECT_NEAR(get_mag_strength_tesla(45, -85) * 1e9, 54320, 145 + 543); - EXPECT_NEAR(get_mag_strength_tesla(45, -80) * 1e9, 53891, 145 + 539); - EXPECT_NEAR(get_mag_strength_tesla(45, -75) * 1e9, 53276, 145 + 533); - EXPECT_NEAR(get_mag_strength_tesla(45, -70) * 1e9, 52515, 145 + 525); - EXPECT_NEAR(get_mag_strength_tesla(45, -65) * 1e9, 51658, 145 + 517); - EXPECT_NEAR(get_mag_strength_tesla(45, -60) * 1e9, 50764, 145 + 508); - EXPECT_NEAR(get_mag_strength_tesla(45, -55) * 1e9, 49887, 145 + 499); - EXPECT_NEAR(get_mag_strength_tesla(45, -50) * 1e9, 49071, 145 + 491); - EXPECT_NEAR(get_mag_strength_tesla(45, -45) * 1e9, 48350, 145 + 483); - EXPECT_NEAR(get_mag_strength_tesla(45, -40) * 1e9, 47740, 145 + 477); - EXPECT_NEAR(get_mag_strength_tesla(45, -35) * 1e9, 47248, 145 + 472); + EXPECT_NEAR(get_mag_strength_tesla(45, -175) * 1e9, 45076, 145 + 451); + EXPECT_NEAR(get_mag_strength_tesla(45, -170) * 1e9, 45117, 145 + 451); + EXPECT_NEAR(get_mag_strength_tesla(45, -165) * 1e9, 45332, 145 + 453); + EXPECT_NEAR(get_mag_strength_tesla(45, -160) * 1e9, 45712, 145 + 457); + EXPECT_NEAR(get_mag_strength_tesla(45, -155) * 1e9, 46242, 145 + 462); + EXPECT_NEAR(get_mag_strength_tesla(45, -150) * 1e9, 46903, 145 + 469); + EXPECT_NEAR(get_mag_strength_tesla(45, -145) * 1e9, 47669, 145 + 477); + EXPECT_NEAR(get_mag_strength_tesla(45, -140) * 1e9, 48509, 145 + 485); + EXPECT_NEAR(get_mag_strength_tesla(45, -135) * 1e9, 49392, 145 + 494); + EXPECT_NEAR(get_mag_strength_tesla(45, -130) * 1e9, 50285, 145 + 503); + EXPECT_NEAR(get_mag_strength_tesla(45, -125) * 1e9, 51161, 145 + 512); + EXPECT_NEAR(get_mag_strength_tesla(45, -120) * 1e9, 51992, 145 + 520); + EXPECT_NEAR(get_mag_strength_tesla(45, -115) * 1e9, 52752, 145 + 528); + EXPECT_NEAR(get_mag_strength_tesla(45, -110) * 1e9, 53415, 145 + 534); + EXPECT_NEAR(get_mag_strength_tesla(45, -105) * 1e9, 53952, 145 + 540); + EXPECT_NEAR(get_mag_strength_tesla(45, -100) * 1e9, 54334, 145 + 543); + EXPECT_NEAR(get_mag_strength_tesla(45, -95) * 1e9, 54533, 145 + 545); + EXPECT_NEAR(get_mag_strength_tesla(45, -90) * 1e9, 54527, 145 + 545); + EXPECT_NEAR(get_mag_strength_tesla(45, -85) * 1e9, 54307, 145 + 543); + EXPECT_NEAR(get_mag_strength_tesla(45, -80) * 1e9, 53878, 145 + 539); + EXPECT_NEAR(get_mag_strength_tesla(45, -75) * 1e9, 53264, 145 + 533); + EXPECT_NEAR(get_mag_strength_tesla(45, -70) * 1e9, 52504, 145 + 525); + EXPECT_NEAR(get_mag_strength_tesla(45, -65) * 1e9, 51649, 145 + 516); + EXPECT_NEAR(get_mag_strength_tesla(45, -60) * 1e9, 50756, 145 + 508); + EXPECT_NEAR(get_mag_strength_tesla(45, -55) * 1e9, 49880, 145 + 499); + EXPECT_NEAR(get_mag_strength_tesla(45, -50) * 1e9, 49066, 145 + 491); + EXPECT_NEAR(get_mag_strength_tesla(45, -45) * 1e9, 48345, 145 + 483); + EXPECT_NEAR(get_mag_strength_tesla(45, -40) * 1e9, 47737, 145 + 477); + EXPECT_NEAR(get_mag_strength_tesla(45, -35) * 1e9, 47246, 145 + 472); EXPECT_NEAR(get_mag_strength_tesla(45, -30) * 1e9, 46876, 145 + 469); - EXPECT_NEAR(get_mag_strength_tesla(45, -25) * 1e9, 46623, 145 + 466); - EXPECT_NEAR(get_mag_strength_tesla(45, -20) * 1e9, 46489, 145 + 465); - EXPECT_NEAR(get_mag_strength_tesla(45, -15) * 1e9, 46470, 145 + 465); - EXPECT_NEAR(get_mag_strength_tesla(45, -10) * 1e9, 46561, 145 + 466); - EXPECT_NEAR(get_mag_strength_tesla(45, -5) * 1e9, 46746, 145 + 467); - EXPECT_NEAR(get_mag_strength_tesla(45, 0) * 1e9, 47005, 145 + 470); - EXPECT_NEAR(get_mag_strength_tesla(45, 5) * 1e9, 47318, 145 + 473); - EXPECT_NEAR(get_mag_strength_tesla(45, 10) * 1e9, 47668, 145 + 477); - EXPECT_NEAR(get_mag_strength_tesla(45, 15) * 1e9, 48047, 145 + 480); - EXPECT_NEAR(get_mag_strength_tesla(45, 20) * 1e9, 48457, 145 + 485); - EXPECT_NEAR(get_mag_strength_tesla(45, 25) * 1e9, 48904, 145 + 489); - EXPECT_NEAR(get_mag_strength_tesla(45, 30) * 1e9, 49399, 145 + 494); - EXPECT_NEAR(get_mag_strength_tesla(45, 35) * 1e9, 49952, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, 40) * 1e9, 50568, 145 + 506); - EXPECT_NEAR(get_mag_strength_tesla(45, 45) * 1e9, 51250, 145 + 513); - EXPECT_NEAR(get_mag_strength_tesla(45, 50) * 1e9, 51998, 145 + 520); - EXPECT_NEAR(get_mag_strength_tesla(45, 55) * 1e9, 52805, 145 + 528); - EXPECT_NEAR(get_mag_strength_tesla(45, 60) * 1e9, 53662, 145 + 537); - EXPECT_NEAR(get_mag_strength_tesla(45, 65) * 1e9, 54549, 145 + 545); - EXPECT_NEAR(get_mag_strength_tesla(45, 70) * 1e9, 55437, 145 + 554); - EXPECT_NEAR(get_mag_strength_tesla(45, 75) * 1e9, 56289, 145 + 563); - EXPECT_NEAR(get_mag_strength_tesla(45, 80) * 1e9, 57063, 145 + 571); - EXPECT_NEAR(get_mag_strength_tesla(45, 85) * 1e9, 57719, 145 + 577); - EXPECT_NEAR(get_mag_strength_tesla(45, 90) * 1e9, 58218, 145 + 582); - EXPECT_NEAR(get_mag_strength_tesla(45, 95) * 1e9, 58527, 145 + 585); - EXPECT_NEAR(get_mag_strength_tesla(45, 100) * 1e9, 58619, 145 + 586); - EXPECT_NEAR(get_mag_strength_tesla(45, 105) * 1e9, 58477, 145 + 585); - EXPECT_NEAR(get_mag_strength_tesla(45, 110) * 1e9, 58090, 145 + 581); - EXPECT_NEAR(get_mag_strength_tesla(45, 115) * 1e9, 57461, 145 + 575); - EXPECT_NEAR(get_mag_strength_tesla(45, 120) * 1e9, 56610, 145 + 566); - EXPECT_NEAR(get_mag_strength_tesla(45, 125) * 1e9, 55570, 145 + 556); - EXPECT_NEAR(get_mag_strength_tesla(45, 130) * 1e9, 54392, 145 + 544); - EXPECT_NEAR(get_mag_strength_tesla(45, 135) * 1e9, 53133, 145 + 531); - EXPECT_NEAR(get_mag_strength_tesla(45, 140) * 1e9, 51852, 145 + 519); - EXPECT_NEAR(get_mag_strength_tesla(45, 145) * 1e9, 50603, 145 + 506); - EXPECT_NEAR(get_mag_strength_tesla(45, 150) * 1e9, 49430, 145 + 494); - EXPECT_NEAR(get_mag_strength_tesla(45, 155) * 1e9, 48363, 145 + 484); - EXPECT_NEAR(get_mag_strength_tesla(45, 160) * 1e9, 47426, 145 + 474); - EXPECT_NEAR(get_mag_strength_tesla(45, 165) * 1e9, 46632, 145 + 466); - EXPECT_NEAR(get_mag_strength_tesla(45, 170) * 1e9, 45992, 145 + 460); - EXPECT_NEAR(get_mag_strength_tesla(45, 175) * 1e9, 45516, 145 + 455); + EXPECT_NEAR(get_mag_strength_tesla(45, -25) * 1e9, 46624, 145 + 466); + EXPECT_NEAR(get_mag_strength_tesla(45, -20) * 1e9, 46490, 145 + 465); + EXPECT_NEAR(get_mag_strength_tesla(45, -15) * 1e9, 46473, 145 + 465); + EXPECT_NEAR(get_mag_strength_tesla(45, -10) * 1e9, 46565, 145 + 466); + EXPECT_NEAR(get_mag_strength_tesla(45, -5) * 1e9, 46751, 145 + 468); + EXPECT_NEAR(get_mag_strength_tesla(45, 0) * 1e9, 47011, 145 + 470); + EXPECT_NEAR(get_mag_strength_tesla(45, 5) * 1e9, 47325, 145 + 473); + EXPECT_NEAR(get_mag_strength_tesla(45, 10) * 1e9, 47676, 145 + 477); + EXPECT_NEAR(get_mag_strength_tesla(45, 15) * 1e9, 48055, 145 + 481); + EXPECT_NEAR(get_mag_strength_tesla(45, 20) * 1e9, 48465, 145 + 485); + EXPECT_NEAR(get_mag_strength_tesla(45, 25) * 1e9, 48913, 145 + 489); + EXPECT_NEAR(get_mag_strength_tesla(45, 30) * 1e9, 49409, 145 + 494); + EXPECT_NEAR(get_mag_strength_tesla(45, 35) * 1e9, 49962, 145 + 500); + EXPECT_NEAR(get_mag_strength_tesla(45, 40) * 1e9, 50579, 145 + 506); + EXPECT_NEAR(get_mag_strength_tesla(45, 45) * 1e9, 51261, 145 + 513); + EXPECT_NEAR(get_mag_strength_tesla(45, 50) * 1e9, 52008, 145 + 520); + EXPECT_NEAR(get_mag_strength_tesla(45, 55) * 1e9, 52816, 145 + 528); + EXPECT_NEAR(get_mag_strength_tesla(45, 60) * 1e9, 53674, 145 + 537); + EXPECT_NEAR(get_mag_strength_tesla(45, 65) * 1e9, 54561, 145 + 546); + EXPECT_NEAR(get_mag_strength_tesla(45, 70) * 1e9, 55449, 145 + 554); + EXPECT_NEAR(get_mag_strength_tesla(45, 75) * 1e9, 56301, 145 + 563); + EXPECT_NEAR(get_mag_strength_tesla(45, 80) * 1e9, 57075, 145 + 571); + EXPECT_NEAR(get_mag_strength_tesla(45, 85) * 1e9, 57730, 145 + 577); + EXPECT_NEAR(get_mag_strength_tesla(45, 90) * 1e9, 58228, 145 + 582); + EXPECT_NEAR(get_mag_strength_tesla(45, 95) * 1e9, 58536, 145 + 585); + EXPECT_NEAR(get_mag_strength_tesla(45, 100) * 1e9, 58628, 145 + 586); + EXPECT_NEAR(get_mag_strength_tesla(45, 105) * 1e9, 58484, 145 + 585); + EXPECT_NEAR(get_mag_strength_tesla(45, 110) * 1e9, 58095, 145 + 581); + EXPECT_NEAR(get_mag_strength_tesla(45, 115) * 1e9, 57466, 145 + 575); + EXPECT_NEAR(get_mag_strength_tesla(45, 120) * 1e9, 56614, 145 + 566); + EXPECT_NEAR(get_mag_strength_tesla(45, 125) * 1e9, 55574, 145 + 556); + EXPECT_NEAR(get_mag_strength_tesla(45, 130) * 1e9, 54396, 145 + 544); + EXPECT_NEAR(get_mag_strength_tesla(45, 135) * 1e9, 53137, 145 + 531); + EXPECT_NEAR(get_mag_strength_tesla(45, 140) * 1e9, 51857, 145 + 519); + EXPECT_NEAR(get_mag_strength_tesla(45, 145) * 1e9, 50608, 145 + 506); + EXPECT_NEAR(get_mag_strength_tesla(45, 150) * 1e9, 49434, 145 + 494); + EXPECT_NEAR(get_mag_strength_tesla(45, 155) * 1e9, 48368, 145 + 484); + EXPECT_NEAR(get_mag_strength_tesla(45, 160) * 1e9, 47430, 145 + 474); + EXPECT_NEAR(get_mag_strength_tesla(45, 165) * 1e9, 46636, 145 + 466); + EXPECT_NEAR(get_mag_strength_tesla(45, 170) * 1e9, 45996, 145 + 460); + EXPECT_NEAR(get_mag_strength_tesla(45, 175) * 1e9, 45518, 145 + 455); EXPECT_NEAR(get_mag_strength_tesla(45, 180) * 1e9, 45210, 145 + 452); EXPECT_NEAR(get_mag_strength_tesla(50, -180) * 1e9, 48320, 145 + 483); - EXPECT_NEAR(get_mag_strength_tesla(50, -175) * 1e9, 48183, 145 + 482); - EXPECT_NEAR(get_mag_strength_tesla(50, -170) * 1e9, 48207, 145 + 482); - EXPECT_NEAR(get_mag_strength_tesla(50, -165) * 1e9, 48391, 145 + 484); - EXPECT_NEAR(get_mag_strength_tesla(50, -160) * 1e9, 48728, 145 + 487); - EXPECT_NEAR(get_mag_strength_tesla(50, -155) * 1e9, 49204, 145 + 492); - EXPECT_NEAR(get_mag_strength_tesla(50, -150) * 1e9, 49799, 145 + 498); - EXPECT_NEAR(get_mag_strength_tesla(50, -145) * 1e9, 50488, 145 + 505); - EXPECT_NEAR(get_mag_strength_tesla(50, -140) * 1e9, 51242, 145 + 512); - EXPECT_NEAR(get_mag_strength_tesla(50, -135) * 1e9, 52031, 145 + 520); - EXPECT_NEAR(get_mag_strength_tesla(50, -130) * 1e9, 52825, 145 + 528); - EXPECT_NEAR(get_mag_strength_tesla(50, -125) * 1e9, 53597, 145 + 536); - EXPECT_NEAR(get_mag_strength_tesla(50, -120) * 1e9, 54321, 145 + 543); - EXPECT_NEAR(get_mag_strength_tesla(50, -115) * 1e9, 54975, 145 + 550); - EXPECT_NEAR(get_mag_strength_tesla(50, -110) * 1e9, 55535, 145 + 555); - EXPECT_NEAR(get_mag_strength_tesla(50, -105) * 1e9, 55978, 145 + 560); - EXPECT_NEAR(get_mag_strength_tesla(50, -100) * 1e9, 56280, 145 + 563); - EXPECT_NEAR(get_mag_strength_tesla(50, -95) * 1e9, 56420, 145 + 564); - EXPECT_NEAR(get_mag_strength_tesla(50, -90) * 1e9, 56382, 145 + 564); - EXPECT_NEAR(get_mag_strength_tesla(50, -85) * 1e9, 56161, 145 + 562); - EXPECT_NEAR(get_mag_strength_tesla(50, -80) * 1e9, 55761, 145 + 558); - EXPECT_NEAR(get_mag_strength_tesla(50, -75) * 1e9, 55200, 145 + 552); - EXPECT_NEAR(get_mag_strength_tesla(50, -70) * 1e9, 54509, 145 + 545); - EXPECT_NEAR(get_mag_strength_tesla(50, -65) * 1e9, 53728, 145 + 537); - EXPECT_NEAR(get_mag_strength_tesla(50, -60) * 1e9, 52902, 145 + 529); - EXPECT_NEAR(get_mag_strength_tesla(50, -55) * 1e9, 52076, 145 + 521); - EXPECT_NEAR(get_mag_strength_tesla(50, -50) * 1e9, 51287, 145 + 513); - EXPECT_NEAR(get_mag_strength_tesla(50, -45) * 1e9, 50566, 145 + 506); - EXPECT_NEAR(get_mag_strength_tesla(50, -40) * 1e9, 49934, 145 + 499); - EXPECT_NEAR(get_mag_strength_tesla(50, -35) * 1e9, 49402, 145 + 494); + EXPECT_NEAR(get_mag_strength_tesla(50, -175) * 1e9, 48181, 145 + 482); + EXPECT_NEAR(get_mag_strength_tesla(50, -170) * 1e9, 48203, 145 + 482); + EXPECT_NEAR(get_mag_strength_tesla(50, -165) * 1e9, 48385, 145 + 484); + EXPECT_NEAR(get_mag_strength_tesla(50, -160) * 1e9, 48720, 145 + 487); + EXPECT_NEAR(get_mag_strength_tesla(50, -155) * 1e9, 49195, 145 + 492); + EXPECT_NEAR(get_mag_strength_tesla(50, -150) * 1e9, 49789, 145 + 498); + EXPECT_NEAR(get_mag_strength_tesla(50, -145) * 1e9, 50477, 145 + 505); + EXPECT_NEAR(get_mag_strength_tesla(50, -140) * 1e9, 51230, 145 + 512); + EXPECT_NEAR(get_mag_strength_tesla(50, -135) * 1e9, 52018, 145 + 520); + EXPECT_NEAR(get_mag_strength_tesla(50, -130) * 1e9, 52811, 145 + 528); + EXPECT_NEAR(get_mag_strength_tesla(50, -125) * 1e9, 53583, 145 + 536); + EXPECT_NEAR(get_mag_strength_tesla(50, -120) * 1e9, 54307, 145 + 543); + EXPECT_NEAR(get_mag_strength_tesla(50, -115) * 1e9, 54961, 145 + 550); + EXPECT_NEAR(get_mag_strength_tesla(50, -110) * 1e9, 55522, 145 + 555); + EXPECT_NEAR(get_mag_strength_tesla(50, -105) * 1e9, 55964, 145 + 560); + EXPECT_NEAR(get_mag_strength_tesla(50, -100) * 1e9, 56267, 145 + 563); + EXPECT_NEAR(get_mag_strength_tesla(50, -95) * 1e9, 56407, 145 + 564); + EXPECT_NEAR(get_mag_strength_tesla(50, -90) * 1e9, 56371, 145 + 564); + EXPECT_NEAR(get_mag_strength_tesla(50, -85) * 1e9, 56150, 145 + 562); + EXPECT_NEAR(get_mag_strength_tesla(50, -80) * 1e9, 55750, 145 + 558); + EXPECT_NEAR(get_mag_strength_tesla(50, -75) * 1e9, 55190, 145 + 552); + EXPECT_NEAR(get_mag_strength_tesla(50, -70) * 1e9, 54500, 145 + 545); + EXPECT_NEAR(get_mag_strength_tesla(50, -65) * 1e9, 53720, 145 + 537); + EXPECT_NEAR(get_mag_strength_tesla(50, -60) * 1e9, 52896, 145 + 529); + EXPECT_NEAR(get_mag_strength_tesla(50, -55) * 1e9, 52070, 145 + 521); + EXPECT_NEAR(get_mag_strength_tesla(50, -50) * 1e9, 51283, 145 + 513); + EXPECT_NEAR(get_mag_strength_tesla(50, -45) * 1e9, 50563, 145 + 506); + EXPECT_NEAR(get_mag_strength_tesla(50, -40) * 1e9, 49932, 145 + 499); + EXPECT_NEAR(get_mag_strength_tesla(50, -35) * 1e9, 49400, 145 + 494); EXPECT_NEAR(get_mag_strength_tesla(50, -30) * 1e9, 48975, 145 + 490); - EXPECT_NEAR(get_mag_strength_tesla(50, -25) * 1e9, 48658, 145 + 487); - EXPECT_NEAR(get_mag_strength_tesla(50, -20) * 1e9, 48451, 145 + 485); - EXPECT_NEAR(get_mag_strength_tesla(50, -15) * 1e9, 48351, 145 + 484); - EXPECT_NEAR(get_mag_strength_tesla(50, -10) * 1e9, 48353, 145 + 484); - EXPECT_NEAR(get_mag_strength_tesla(50, -5) * 1e9, 48445, 145 + 484); - EXPECT_NEAR(get_mag_strength_tesla(50, 0) * 1e9, 48614, 145 + 486); - EXPECT_NEAR(get_mag_strength_tesla(50, 5) * 1e9, 48846, 145 + 488); - EXPECT_NEAR(get_mag_strength_tesla(50, 10) * 1e9, 49128, 145 + 491); - EXPECT_NEAR(get_mag_strength_tesla(50, 15) * 1e9, 49457, 145 + 495); - EXPECT_NEAR(get_mag_strength_tesla(50, 20) * 1e9, 49832, 145 + 498); - EXPECT_NEAR(get_mag_strength_tesla(50, 25) * 1e9, 50260, 145 + 503); - EXPECT_NEAR(get_mag_strength_tesla(50, 30) * 1e9, 50750, 145 + 508); - EXPECT_NEAR(get_mag_strength_tesla(50, 35) * 1e9, 51311, 145 + 513); - EXPECT_NEAR(get_mag_strength_tesla(50, 40) * 1e9, 51947, 145 + 519); - EXPECT_NEAR(get_mag_strength_tesla(50, 45) * 1e9, 52662, 145 + 527); - EXPECT_NEAR(get_mag_strength_tesla(50, 50) * 1e9, 53452, 145 + 535); - EXPECT_NEAR(get_mag_strength_tesla(50, 55) * 1e9, 54308, 145 + 543); - EXPECT_NEAR(get_mag_strength_tesla(50, 60) * 1e9, 55212, 145 + 552); - EXPECT_NEAR(get_mag_strength_tesla(50, 65) * 1e9, 56139, 145 + 561); - EXPECT_NEAR(get_mag_strength_tesla(50, 70) * 1e9, 57057, 145 + 571); - EXPECT_NEAR(get_mag_strength_tesla(50, 75) * 1e9, 57930, 145 + 579); - EXPECT_NEAR(get_mag_strength_tesla(50, 80) * 1e9, 58716, 145 + 587); - EXPECT_NEAR(get_mag_strength_tesla(50, 85) * 1e9, 59378, 145 + 594); - EXPECT_NEAR(get_mag_strength_tesla(50, 90) * 1e9, 59880, 145 + 599); - EXPECT_NEAR(get_mag_strength_tesla(50, 95) * 1e9, 60193, 145 + 602); - EXPECT_NEAR(get_mag_strength_tesla(50, 100) * 1e9, 60296, 145 + 603); - EXPECT_NEAR(get_mag_strength_tesla(50, 105) * 1e9, 60176, 145 + 602); - EXPECT_NEAR(get_mag_strength_tesla(50, 110) * 1e9, 59831, 145 + 598); - EXPECT_NEAR(get_mag_strength_tesla(50, 115) * 1e9, 59269, 145 + 593); - EXPECT_NEAR(get_mag_strength_tesla(50, 120) * 1e9, 58511, 145 + 585); - EXPECT_NEAR(get_mag_strength_tesla(50, 125) * 1e9, 57592, 145 + 576); - EXPECT_NEAR(get_mag_strength_tesla(50, 130) * 1e9, 56554, 145 + 566); - EXPECT_NEAR(get_mag_strength_tesla(50, 135) * 1e9, 55447, 145 + 554); - EXPECT_NEAR(get_mag_strength_tesla(50, 140) * 1e9, 54319, 145 + 543); - EXPECT_NEAR(get_mag_strength_tesla(50, 145) * 1e9, 53215, 145 + 532); - EXPECT_NEAR(get_mag_strength_tesla(50, 150) * 1e9, 52172, 145 + 522); - EXPECT_NEAR(get_mag_strength_tesla(50, 155) * 1e9, 51218, 145 + 512); - EXPECT_NEAR(get_mag_strength_tesla(50, 160) * 1e9, 50373, 145 + 504); - EXPECT_NEAR(get_mag_strength_tesla(50, 165) * 1e9, 49650, 145 + 497); - EXPECT_NEAR(get_mag_strength_tesla(50, 170) * 1e9, 49061, 145 + 491); - EXPECT_NEAR(get_mag_strength_tesla(50, 175) * 1e9, 48615, 145 + 486); + EXPECT_NEAR(get_mag_strength_tesla(50, -25) * 1e9, 48659, 145 + 487); + EXPECT_NEAR(get_mag_strength_tesla(50, -20) * 1e9, 48453, 145 + 485); + EXPECT_NEAR(get_mag_strength_tesla(50, -15) * 1e9, 48354, 145 + 484); + EXPECT_NEAR(get_mag_strength_tesla(50, -10) * 1e9, 48357, 145 + 484); + EXPECT_NEAR(get_mag_strength_tesla(50, -5) * 1e9, 48450, 145 + 484); + EXPECT_NEAR(get_mag_strength_tesla(50, 0) * 1e9, 48620, 145 + 486); + EXPECT_NEAR(get_mag_strength_tesla(50, 5) * 1e9, 48852, 145 + 489); + EXPECT_NEAR(get_mag_strength_tesla(50, 10) * 1e9, 49136, 145 + 491); + EXPECT_NEAR(get_mag_strength_tesla(50, 15) * 1e9, 49465, 145 + 495); + EXPECT_NEAR(get_mag_strength_tesla(50, 20) * 1e9, 49840, 145 + 498); + EXPECT_NEAR(get_mag_strength_tesla(50, 25) * 1e9, 50269, 145 + 503); + EXPECT_NEAR(get_mag_strength_tesla(50, 30) * 1e9, 50760, 145 + 508); + EXPECT_NEAR(get_mag_strength_tesla(50, 35) * 1e9, 51320, 145 + 513); + EXPECT_NEAR(get_mag_strength_tesla(50, 40) * 1e9, 51958, 145 + 520); + EXPECT_NEAR(get_mag_strength_tesla(50, 45) * 1e9, 52673, 145 + 527); + EXPECT_NEAR(get_mag_strength_tesla(50, 50) * 1e9, 53463, 145 + 535); + EXPECT_NEAR(get_mag_strength_tesla(50, 55) * 1e9, 54319, 145 + 543); + EXPECT_NEAR(get_mag_strength_tesla(50, 60) * 1e9, 55223, 145 + 552); + EXPECT_NEAR(get_mag_strength_tesla(50, 65) * 1e9, 56150, 145 + 562); + EXPECT_NEAR(get_mag_strength_tesla(50, 70) * 1e9, 57069, 145 + 571); + EXPECT_NEAR(get_mag_strength_tesla(50, 75) * 1e9, 57941, 145 + 579); + EXPECT_NEAR(get_mag_strength_tesla(50, 80) * 1e9, 58727, 145 + 587); + EXPECT_NEAR(get_mag_strength_tesla(50, 85) * 1e9, 59388, 145 + 594); + EXPECT_NEAR(get_mag_strength_tesla(50, 90) * 1e9, 59889, 145 + 599); + EXPECT_NEAR(get_mag_strength_tesla(50, 95) * 1e9, 60201, 145 + 602); + EXPECT_NEAR(get_mag_strength_tesla(50, 100) * 1e9, 60303, 145 + 603); + EXPECT_NEAR(get_mag_strength_tesla(50, 105) * 1e9, 60182, 145 + 602); + EXPECT_NEAR(get_mag_strength_tesla(50, 110) * 1e9, 59835, 145 + 598); + EXPECT_NEAR(get_mag_strength_tesla(50, 115) * 1e9, 59272, 145 + 593); + EXPECT_NEAR(get_mag_strength_tesla(50, 120) * 1e9, 58514, 145 + 585); + EXPECT_NEAR(get_mag_strength_tesla(50, 125) * 1e9, 57595, 145 + 576); + EXPECT_NEAR(get_mag_strength_tesla(50, 130) * 1e9, 56557, 145 + 566); + EXPECT_NEAR(get_mag_strength_tesla(50, 135) * 1e9, 55450, 145 + 555); + EXPECT_NEAR(get_mag_strength_tesla(50, 140) * 1e9, 54323, 145 + 543); + EXPECT_NEAR(get_mag_strength_tesla(50, 145) * 1e9, 53220, 145 + 532); + EXPECT_NEAR(get_mag_strength_tesla(50, 150) * 1e9, 52178, 145 + 522); + EXPECT_NEAR(get_mag_strength_tesla(50, 155) * 1e9, 51224, 145 + 512); + EXPECT_NEAR(get_mag_strength_tesla(50, 160) * 1e9, 50378, 145 + 504); + EXPECT_NEAR(get_mag_strength_tesla(50, 165) * 1e9, 49655, 145 + 497); + EXPECT_NEAR(get_mag_strength_tesla(50, 170) * 1e9, 49065, 145 + 491); + EXPECT_NEAR(get_mag_strength_tesla(50, 175) * 1e9, 48617, 145 + 486); EXPECT_NEAR(get_mag_strength_tesla(50, 180) * 1e9, 48320, 145 + 483); - EXPECT_NEAR(get_mag_strength_tesla(55, -180) * 1e9, 51312, 145 + 513); - EXPECT_NEAR(get_mag_strength_tesla(55, -175) * 1e9, 51177, 145 + 512); - EXPECT_NEAR(get_mag_strength_tesla(55, -170) * 1e9, 51176, 145 + 512); - EXPECT_NEAR(get_mag_strength_tesla(55, -165) * 1e9, 51311, 145 + 513); - EXPECT_NEAR(get_mag_strength_tesla(55, -160) * 1e9, 51574, 145 + 516); - EXPECT_NEAR(get_mag_strength_tesla(55, -155) * 1e9, 51954, 145 + 520); - EXPECT_NEAR(get_mag_strength_tesla(55, -150) * 1e9, 52434, 145 + 524); - EXPECT_NEAR(get_mag_strength_tesla(55, -145) * 1e9, 52994, 145 + 530); - EXPECT_NEAR(get_mag_strength_tesla(55, -140) * 1e9, 53608, 145 + 536); - EXPECT_NEAR(get_mag_strength_tesla(55, -135) * 1e9, 54250, 145 + 543); - EXPECT_NEAR(get_mag_strength_tesla(55, -130) * 1e9, 54897, 145 + 549); - EXPECT_NEAR(get_mag_strength_tesla(55, -125) * 1e9, 55522, 145 + 555); - EXPECT_NEAR(get_mag_strength_tesla(55, -120) * 1e9, 56105, 145 + 561); - EXPECT_NEAR(get_mag_strength_tesla(55, -115) * 1e9, 56625, 145 + 566); - EXPECT_NEAR(get_mag_strength_tesla(55, -110) * 1e9, 57061, 145 + 571); - EXPECT_NEAR(get_mag_strength_tesla(55, -105) * 1e9, 57394, 145 + 574); - EXPECT_NEAR(get_mag_strength_tesla(55, -100) * 1e9, 57608, 145 + 576); - EXPECT_NEAR(get_mag_strength_tesla(55, -95) * 1e9, 57686, 145 + 577); - EXPECT_NEAR(get_mag_strength_tesla(55, -90) * 1e9, 57618, 145 + 576); - EXPECT_NEAR(get_mag_strength_tesla(55, -85) * 1e9, 57400, 145 + 574); - EXPECT_NEAR(get_mag_strength_tesla(55, -80) * 1e9, 57037, 145 + 570); - EXPECT_NEAR(get_mag_strength_tesla(55, -75) * 1e9, 56542, 145 + 565); - EXPECT_NEAR(get_mag_strength_tesla(55, -70) * 1e9, 55937, 145 + 559); - EXPECT_NEAR(get_mag_strength_tesla(55, -65) * 1e9, 55252, 145 + 553); - EXPECT_NEAR(get_mag_strength_tesla(55, -60) * 1e9, 54520, 145 + 545); - EXPECT_NEAR(get_mag_strength_tesla(55, -55) * 1e9, 53775, 145 + 538); - EXPECT_NEAR(get_mag_strength_tesla(55, -50) * 1e9, 53047, 145 + 530); - EXPECT_NEAR(get_mag_strength_tesla(55, -45) * 1e9, 52364, 145 + 524); - EXPECT_NEAR(get_mag_strength_tesla(55, -40) * 1e9, 51745, 145 + 517); + EXPECT_NEAR(get_mag_strength_tesla(55, -180) * 1e9, 51313, 145 + 513); + EXPECT_NEAR(get_mag_strength_tesla(55, -175) * 1e9, 51175, 145 + 512); + EXPECT_NEAR(get_mag_strength_tesla(55, -170) * 1e9, 51173, 145 + 512); + EXPECT_NEAR(get_mag_strength_tesla(55, -165) * 1e9, 51305, 145 + 513); + EXPECT_NEAR(get_mag_strength_tesla(55, -160) * 1e9, 51566, 145 + 516); + EXPECT_NEAR(get_mag_strength_tesla(55, -155) * 1e9, 51944, 145 + 519); + EXPECT_NEAR(get_mag_strength_tesla(55, -150) * 1e9, 52423, 145 + 524); + EXPECT_NEAR(get_mag_strength_tesla(55, -145) * 1e9, 52982, 145 + 530); + EXPECT_NEAR(get_mag_strength_tesla(55, -140) * 1e9, 53595, 145 + 536); + EXPECT_NEAR(get_mag_strength_tesla(55, -135) * 1e9, 54237, 145 + 542); + EXPECT_NEAR(get_mag_strength_tesla(55, -130) * 1e9, 54883, 145 + 549); + EXPECT_NEAR(get_mag_strength_tesla(55, -125) * 1e9, 55509, 145 + 555); + EXPECT_NEAR(get_mag_strength_tesla(55, -120) * 1e9, 56092, 145 + 561); + EXPECT_NEAR(get_mag_strength_tesla(55, -115) * 1e9, 56612, 145 + 566); + EXPECT_NEAR(get_mag_strength_tesla(55, -110) * 1e9, 57048, 145 + 570); + EXPECT_NEAR(get_mag_strength_tesla(55, -105) * 1e9, 57382, 145 + 574); + EXPECT_NEAR(get_mag_strength_tesla(55, -100) * 1e9, 57596, 145 + 576); + EXPECT_NEAR(get_mag_strength_tesla(55, -95) * 1e9, 57675, 145 + 577); + EXPECT_NEAR(get_mag_strength_tesla(55, -90) * 1e9, 57608, 145 + 576); + EXPECT_NEAR(get_mag_strength_tesla(55, -85) * 1e9, 57391, 145 + 574); + EXPECT_NEAR(get_mag_strength_tesla(55, -80) * 1e9, 57029, 145 + 570); + EXPECT_NEAR(get_mag_strength_tesla(55, -75) * 1e9, 56534, 145 + 565); + EXPECT_NEAR(get_mag_strength_tesla(55, -70) * 1e9, 55930, 145 + 559); + EXPECT_NEAR(get_mag_strength_tesla(55, -65) * 1e9, 55246, 145 + 552); + EXPECT_NEAR(get_mag_strength_tesla(55, -60) * 1e9, 54515, 145 + 545); + EXPECT_NEAR(get_mag_strength_tesla(55, -55) * 1e9, 53771, 145 + 538); + EXPECT_NEAR(get_mag_strength_tesla(55, -50) * 1e9, 53044, 145 + 530); + EXPECT_NEAR(get_mag_strength_tesla(55, -45) * 1e9, 52362, 145 + 524); + EXPECT_NEAR(get_mag_strength_tesla(55, -40) * 1e9, 51744, 145 + 517); EXPECT_NEAR(get_mag_strength_tesla(55, -35) * 1e9, 51204, 145 + 512); EXPECT_NEAR(get_mag_strength_tesla(55, -30) * 1e9, 50752, 145 + 508); - EXPECT_NEAR(get_mag_strength_tesla(55, -25) * 1e9, 50393, 145 + 504); - EXPECT_NEAR(get_mag_strength_tesla(55, -20) * 1e9, 50130, 145 + 501); - EXPECT_NEAR(get_mag_strength_tesla(55, -15) * 1e9, 49964, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, -10) * 1e9, 49889, 145 + 499); - EXPECT_NEAR(get_mag_strength_tesla(55, -5) * 1e9, 49901, 145 + 499); - EXPECT_NEAR(get_mag_strength_tesla(55, 0) * 1e9, 49991, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, 5) * 1e9, 50150, 145 + 502); - EXPECT_NEAR(get_mag_strength_tesla(55, 10) * 1e9, 50374, 145 + 504); - EXPECT_NEAR(get_mag_strength_tesla(55, 15) * 1e9, 50659, 145 + 507); - EXPECT_NEAR(get_mag_strength_tesla(55, 20) * 1e9, 51006, 145 + 510); - EXPECT_NEAR(get_mag_strength_tesla(55, 25) * 1e9, 51420, 145 + 514); - EXPECT_NEAR(get_mag_strength_tesla(55, 30) * 1e9, 51907, 145 + 519); - EXPECT_NEAR(get_mag_strength_tesla(55, 35) * 1e9, 52473, 145 + 525); - EXPECT_NEAR(get_mag_strength_tesla(55, 40) * 1e9, 53121, 145 + 531); - EXPECT_NEAR(get_mag_strength_tesla(55, 45) * 1e9, 53849, 145 + 538); - EXPECT_NEAR(get_mag_strength_tesla(55, 50) * 1e9, 54651, 145 + 547); - EXPECT_NEAR(get_mag_strength_tesla(55, 55) * 1e9, 55514, 145 + 555); - EXPECT_NEAR(get_mag_strength_tesla(55, 60) * 1e9, 56417, 145 + 564); - EXPECT_NEAR(get_mag_strength_tesla(55, 65) * 1e9, 57335, 145 + 573); - EXPECT_NEAR(get_mag_strength_tesla(55, 70) * 1e9, 58234, 145 + 582); - EXPECT_NEAR(get_mag_strength_tesla(55, 75) * 1e9, 59080, 145 + 591); - EXPECT_NEAR(get_mag_strength_tesla(55, 80) * 1e9, 59838, 145 + 598); - EXPECT_NEAR(get_mag_strength_tesla(55, 85) * 1e9, 60473, 145 + 605); - EXPECT_NEAR(get_mag_strength_tesla(55, 90) * 1e9, 60955, 145 + 610); - EXPECT_NEAR(get_mag_strength_tesla(55, 95) * 1e9, 61262, 145 + 613); - EXPECT_NEAR(get_mag_strength_tesla(55, 100) * 1e9, 61376, 145 + 614); - EXPECT_NEAR(get_mag_strength_tesla(55, 105) * 1e9, 61292, 145 + 613); - EXPECT_NEAR(get_mag_strength_tesla(55, 110) * 1e9, 61010, 145 + 610); - EXPECT_NEAR(get_mag_strength_tesla(55, 115) * 1e9, 60543, 145 + 605); - EXPECT_NEAR(get_mag_strength_tesla(55, 120) * 1e9, 59913, 145 + 599); - EXPECT_NEAR(get_mag_strength_tesla(55, 125) * 1e9, 59149, 145 + 591); - EXPECT_NEAR(get_mag_strength_tesla(55, 130) * 1e9, 58287, 145 + 583); - EXPECT_NEAR(get_mag_strength_tesla(55, 135) * 1e9, 57367, 145 + 574); - EXPECT_NEAR(get_mag_strength_tesla(55, 140) * 1e9, 56429, 145 + 564); - EXPECT_NEAR(get_mag_strength_tesla(55, 145) * 1e9, 55506, 145 + 555); - EXPECT_NEAR(get_mag_strength_tesla(55, 150) * 1e9, 54631, 145 + 546); - EXPECT_NEAR(get_mag_strength_tesla(55, 155) * 1e9, 53824, 145 + 538); - EXPECT_NEAR(get_mag_strength_tesla(55, 160) * 1e9, 53104, 145 + 531); - EXPECT_NEAR(get_mag_strength_tesla(55, 165) * 1e9, 52483, 145 + 525); - EXPECT_NEAR(get_mag_strength_tesla(55, 170) * 1e9, 51972, 145 + 520); - EXPECT_NEAR(get_mag_strength_tesla(55, 175) * 1e9, 51579, 145 + 516); - EXPECT_NEAR(get_mag_strength_tesla(55, 180) * 1e9, 51312, 145 + 513); + EXPECT_NEAR(get_mag_strength_tesla(55, -25) * 1e9, 50394, 145 + 504); + EXPECT_NEAR(get_mag_strength_tesla(55, -20) * 1e9, 50133, 145 + 501); + EXPECT_NEAR(get_mag_strength_tesla(55, -15) * 1e9, 49967, 145 + 500); + EXPECT_NEAR(get_mag_strength_tesla(55, -10) * 1e9, 49893, 145 + 499); + EXPECT_NEAR(get_mag_strength_tesla(55, -5) * 1e9, 49906, 145 + 499); + EXPECT_NEAR(get_mag_strength_tesla(55, 0) * 1e9, 49996, 145 + 500); + EXPECT_NEAR(get_mag_strength_tesla(55, 5) * 1e9, 50157, 145 + 502); + EXPECT_NEAR(get_mag_strength_tesla(55, 10) * 1e9, 50381, 145 + 504); + EXPECT_NEAR(get_mag_strength_tesla(55, 15) * 1e9, 50666, 145 + 507); + EXPECT_NEAR(get_mag_strength_tesla(55, 20) * 1e9, 51014, 145 + 510); + EXPECT_NEAR(get_mag_strength_tesla(55, 25) * 1e9, 51428, 145 + 514); + EXPECT_NEAR(get_mag_strength_tesla(55, 30) * 1e9, 51916, 145 + 519); + EXPECT_NEAR(get_mag_strength_tesla(55, 35) * 1e9, 52482, 145 + 525); + EXPECT_NEAR(get_mag_strength_tesla(55, 40) * 1e9, 53130, 145 + 531); + EXPECT_NEAR(get_mag_strength_tesla(55, 45) * 1e9, 53859, 145 + 539); + EXPECT_NEAR(get_mag_strength_tesla(55, 50) * 1e9, 54662, 145 + 547); + EXPECT_NEAR(get_mag_strength_tesla(55, 55) * 1e9, 55525, 145 + 555); + EXPECT_NEAR(get_mag_strength_tesla(55, 60) * 1e9, 56428, 145 + 564); + EXPECT_NEAR(get_mag_strength_tesla(55, 65) * 1e9, 57346, 145 + 573); + EXPECT_NEAR(get_mag_strength_tesla(55, 70) * 1e9, 58245, 145 + 582); + EXPECT_NEAR(get_mag_strength_tesla(55, 75) * 1e9, 59091, 145 + 591); + EXPECT_NEAR(get_mag_strength_tesla(55, 80) * 1e9, 59848, 145 + 598); + EXPECT_NEAR(get_mag_strength_tesla(55, 85) * 1e9, 60482, 145 + 605); + EXPECT_NEAR(get_mag_strength_tesla(55, 90) * 1e9, 60963, 145 + 610); + EXPECT_NEAR(get_mag_strength_tesla(55, 95) * 1e9, 61268, 145 + 613); + EXPECT_NEAR(get_mag_strength_tesla(55, 100) * 1e9, 61382, 145 + 614); + EXPECT_NEAR(get_mag_strength_tesla(55, 105) * 1e9, 61296, 145 + 613); + EXPECT_NEAR(get_mag_strength_tesla(55, 110) * 1e9, 61013, 145 + 610); + EXPECT_NEAR(get_mag_strength_tesla(55, 115) * 1e9, 60546, 145 + 605); + EXPECT_NEAR(get_mag_strength_tesla(55, 120) * 1e9, 59915, 145 + 599); + EXPECT_NEAR(get_mag_strength_tesla(55, 125) * 1e9, 59151, 145 + 592); + EXPECT_NEAR(get_mag_strength_tesla(55, 130) * 1e9, 58290, 145 + 583); + EXPECT_NEAR(get_mag_strength_tesla(55, 135) * 1e9, 57371, 145 + 574); + EXPECT_NEAR(get_mag_strength_tesla(55, 140) * 1e9, 56433, 145 + 564); + EXPECT_NEAR(get_mag_strength_tesla(55, 145) * 1e9, 55512, 145 + 555); + EXPECT_NEAR(get_mag_strength_tesla(55, 150) * 1e9, 54636, 145 + 546); + EXPECT_NEAR(get_mag_strength_tesla(55, 155) * 1e9, 53830, 145 + 538); + EXPECT_NEAR(get_mag_strength_tesla(55, 160) * 1e9, 53110, 145 + 531); + EXPECT_NEAR(get_mag_strength_tesla(55, 165) * 1e9, 52488, 145 + 525); + EXPECT_NEAR(get_mag_strength_tesla(55, 170) * 1e9, 51976, 145 + 520); + EXPECT_NEAR(get_mag_strength_tesla(55, 175) * 1e9, 51582, 145 + 516); + EXPECT_NEAR(get_mag_strength_tesla(55, 180) * 1e9, 51313, 145 + 513); EXPECT_NEAR(get_mag_strength_tesla(60, -180) * 1e9, 53927, 145 + 539); - EXPECT_NEAR(get_mag_strength_tesla(60, -175) * 1e9, 53793, 145 + 538); - EXPECT_NEAR(get_mag_strength_tesla(60, -170) * 1e9, 53762, 145 + 538); - EXPECT_NEAR(get_mag_strength_tesla(60, -165) * 1e9, 53834, 145 + 538); - EXPECT_NEAR(get_mag_strength_tesla(60, -160) * 1e9, 54004, 145 + 540); - EXPECT_NEAR(get_mag_strength_tesla(60, -155) * 1e9, 54264, 145 + 543); - EXPECT_NEAR(get_mag_strength_tesla(60, -150) * 1e9, 54601, 145 + 546); - EXPECT_NEAR(get_mag_strength_tesla(60, -145) * 1e9, 55000, 145 + 550); - EXPECT_NEAR(get_mag_strength_tesla(60, -140) * 1e9, 55442, 145 + 554); - EXPECT_NEAR(get_mag_strength_tesla(60, -135) * 1e9, 55908, 145 + 559); - EXPECT_NEAR(get_mag_strength_tesla(60, -130) * 1e9, 56378, 145 + 564); - EXPECT_NEAR(get_mag_strength_tesla(60, -125) * 1e9, 56833, 145 + 568); - EXPECT_NEAR(get_mag_strength_tesla(60, -120) * 1e9, 57254, 145 + 573); - EXPECT_NEAR(get_mag_strength_tesla(60, -115) * 1e9, 57624, 145 + 576); - EXPECT_NEAR(get_mag_strength_tesla(60, -110) * 1e9, 57928, 145 + 579); - EXPECT_NEAR(get_mag_strength_tesla(60, -105) * 1e9, 58149, 145 + 581); - EXPECT_NEAR(get_mag_strength_tesla(60, -100) * 1e9, 58276, 145 + 583); - EXPECT_NEAR(get_mag_strength_tesla(60, -95) * 1e9, 58297, 145 + 583); - EXPECT_NEAR(get_mag_strength_tesla(60, -90) * 1e9, 58206, 145 + 582); - EXPECT_NEAR(get_mag_strength_tesla(60, -85) * 1e9, 58000, 145 + 580); - EXPECT_NEAR(get_mag_strength_tesla(60, -80) * 1e9, 57682, 145 + 577); - EXPECT_NEAR(get_mag_strength_tesla(60, -75) * 1e9, 57263, 145 + 573); - EXPECT_NEAR(get_mag_strength_tesla(60, -70) * 1e9, 56757, 145 + 568); - EXPECT_NEAR(get_mag_strength_tesla(60, -65) * 1e9, 56184, 145 + 562); - EXPECT_NEAR(get_mag_strength_tesla(60, -60) * 1e9, 55567, 145 + 556); - EXPECT_NEAR(get_mag_strength_tesla(60, -55) * 1e9, 54931, 145 + 549); - EXPECT_NEAR(get_mag_strength_tesla(60, -50) * 1e9, 54298, 145 + 543); + EXPECT_NEAR(get_mag_strength_tesla(60, -175) * 1e9, 53791, 145 + 538); + EXPECT_NEAR(get_mag_strength_tesla(60, -170) * 1e9, 53758, 145 + 538); + EXPECT_NEAR(get_mag_strength_tesla(60, -165) * 1e9, 53828, 145 + 538); + EXPECT_NEAR(get_mag_strength_tesla(60, -160) * 1e9, 53996, 145 + 540); + EXPECT_NEAR(get_mag_strength_tesla(60, -155) * 1e9, 54255, 145 + 543); + EXPECT_NEAR(get_mag_strength_tesla(60, -150) * 1e9, 54591, 145 + 546); + EXPECT_NEAR(get_mag_strength_tesla(60, -145) * 1e9, 54988, 145 + 550); + EXPECT_NEAR(get_mag_strength_tesla(60, -140) * 1e9, 55430, 145 + 554); + EXPECT_NEAR(get_mag_strength_tesla(60, -135) * 1e9, 55896, 145 + 559); + EXPECT_NEAR(get_mag_strength_tesla(60, -130) * 1e9, 56365, 145 + 564); + EXPECT_NEAR(get_mag_strength_tesla(60, -125) * 1e9, 56820, 145 + 568); + EXPECT_NEAR(get_mag_strength_tesla(60, -120) * 1e9, 57241, 145 + 572); + EXPECT_NEAR(get_mag_strength_tesla(60, -115) * 1e9, 57612, 145 + 576); + EXPECT_NEAR(get_mag_strength_tesla(60, -110) * 1e9, 57916, 145 + 579); + EXPECT_NEAR(get_mag_strength_tesla(60, -105) * 1e9, 58139, 145 + 581); + EXPECT_NEAR(get_mag_strength_tesla(60, -100) * 1e9, 58266, 145 + 583); + EXPECT_NEAR(get_mag_strength_tesla(60, -95) * 1e9, 58288, 145 + 583); + EXPECT_NEAR(get_mag_strength_tesla(60, -90) * 1e9, 58198, 145 + 582); + EXPECT_NEAR(get_mag_strength_tesla(60, -85) * 1e9, 57993, 145 + 580); + EXPECT_NEAR(get_mag_strength_tesla(60, -80) * 1e9, 57676, 145 + 577); + EXPECT_NEAR(get_mag_strength_tesla(60, -75) * 1e9, 57258, 145 + 573); + EXPECT_NEAR(get_mag_strength_tesla(60, -70) * 1e9, 56752, 145 + 568); + EXPECT_NEAR(get_mag_strength_tesla(60, -65) * 1e9, 56180, 145 + 562); + EXPECT_NEAR(get_mag_strength_tesla(60, -60) * 1e9, 55564, 145 + 556); + EXPECT_NEAR(get_mag_strength_tesla(60, -55) * 1e9, 54928, 145 + 549); + EXPECT_NEAR(get_mag_strength_tesla(60, -50) * 1e9, 54297, 145 + 543); EXPECT_NEAR(get_mag_strength_tesla(60, -45) * 1e9, 53690, 145 + 537); EXPECT_NEAR(get_mag_strength_tesla(60, -40) * 1e9, 53125, 145 + 531); - EXPECT_NEAR(get_mag_strength_tesla(60, -35) * 1e9, 52616, 145 + 526); - EXPECT_NEAR(get_mag_strength_tesla(60, -30) * 1e9, 52175, 145 + 522); - EXPECT_NEAR(get_mag_strength_tesla(60, -25) * 1e9, 51809, 145 + 518); - EXPECT_NEAR(get_mag_strength_tesla(60, -20) * 1e9, 51523, 145 + 515); - EXPECT_NEAR(get_mag_strength_tesla(60, -15) * 1e9, 51318, 145 + 513); - EXPECT_NEAR(get_mag_strength_tesla(60, -10) * 1e9, 51196, 145 + 512); - EXPECT_NEAR(get_mag_strength_tesla(60, -5) * 1e9, 51153, 145 + 512); - EXPECT_NEAR(get_mag_strength_tesla(60, 0) * 1e9, 51186, 145 + 512); - EXPECT_NEAR(get_mag_strength_tesla(60, 5) * 1e9, 51294, 145 + 513); - EXPECT_NEAR(get_mag_strength_tesla(60, 10) * 1e9, 51473, 145 + 515); - EXPECT_NEAR(get_mag_strength_tesla(60, 15) * 1e9, 51724, 145 + 517); - EXPECT_NEAR(get_mag_strength_tesla(60, 20) * 1e9, 52046, 145 + 520); - EXPECT_NEAR(get_mag_strength_tesla(60, 25) * 1e9, 52444, 145 + 524); - EXPECT_NEAR(get_mag_strength_tesla(60, 30) * 1e9, 52919, 145 + 529); - EXPECT_NEAR(get_mag_strength_tesla(60, 35) * 1e9, 53473, 145 + 535); - EXPECT_NEAR(get_mag_strength_tesla(60, 40) * 1e9, 54106, 145 + 541); - EXPECT_NEAR(get_mag_strength_tesla(60, 45) * 1e9, 54812, 145 + 548); - EXPECT_NEAR(get_mag_strength_tesla(60, 50) * 1e9, 55582, 145 + 556); - EXPECT_NEAR(get_mag_strength_tesla(60, 55) * 1e9, 56401, 145 + 564); - EXPECT_NEAR(get_mag_strength_tesla(60, 60) * 1e9, 57249, 145 + 572); - EXPECT_NEAR(get_mag_strength_tesla(60, 65) * 1e9, 58100, 145 + 581); - EXPECT_NEAR(get_mag_strength_tesla(60, 70) * 1e9, 58927, 145 + 589); - EXPECT_NEAR(get_mag_strength_tesla(60, 75) * 1e9, 59700, 145 + 597); - EXPECT_NEAR(get_mag_strength_tesla(60, 80) * 1e9, 60389, 145 + 604); - EXPECT_NEAR(get_mag_strength_tesla(60, 85) * 1e9, 60967, 145 + 610); - EXPECT_NEAR(get_mag_strength_tesla(60, 90) * 1e9, 61410, 145 + 614); - EXPECT_NEAR(get_mag_strength_tesla(60, 95) * 1e9, 61702, 145 + 617); - EXPECT_NEAR(get_mag_strength_tesla(60, 100) * 1e9, 61830, 145 + 618); - EXPECT_NEAR(get_mag_strength_tesla(60, 105) * 1e9, 61793, 145 + 618); - EXPECT_NEAR(get_mag_strength_tesla(60, 110) * 1e9, 61593, 145 + 616); - EXPECT_NEAR(get_mag_strength_tesla(60, 115) * 1e9, 61244, 145 + 612); - EXPECT_NEAR(get_mag_strength_tesla(60, 120) * 1e9, 60764, 145 + 608); - EXPECT_NEAR(get_mag_strength_tesla(60, 125) * 1e9, 60176, 145 + 602); - EXPECT_NEAR(get_mag_strength_tesla(60, 130) * 1e9, 59511, 145 + 595); - EXPECT_NEAR(get_mag_strength_tesla(60, 135) * 1e9, 58798, 145 + 588); - EXPECT_NEAR(get_mag_strength_tesla(60, 140) * 1e9, 58065, 145 + 581); - EXPECT_NEAR(get_mag_strength_tesla(60, 145) * 1e9, 57341, 145 + 573); - EXPECT_NEAR(get_mag_strength_tesla(60, 150) * 1e9, 56647, 145 + 566); - EXPECT_NEAR(get_mag_strength_tesla(60, 155) * 1e9, 56003, 145 + 560); - EXPECT_NEAR(get_mag_strength_tesla(60, 160) * 1e9, 55422, 145 + 554); - EXPECT_NEAR(get_mag_strength_tesla(60, 165) * 1e9, 54916, 145 + 549); - EXPECT_NEAR(get_mag_strength_tesla(60, 170) * 1e9, 54494, 145 + 545); - EXPECT_NEAR(get_mag_strength_tesla(60, 175) * 1e9, 54162, 145 + 542); + EXPECT_NEAR(get_mag_strength_tesla(60, -35) * 1e9, 52617, 145 + 526); + EXPECT_NEAR(get_mag_strength_tesla(60, -30) * 1e9, 52177, 145 + 522); + EXPECT_NEAR(get_mag_strength_tesla(60, -25) * 1e9, 51812, 145 + 518); + EXPECT_NEAR(get_mag_strength_tesla(60, -20) * 1e9, 51526, 145 + 515); + EXPECT_NEAR(get_mag_strength_tesla(60, -15) * 1e9, 51322, 145 + 513); + EXPECT_NEAR(get_mag_strength_tesla(60, -10) * 1e9, 51200, 145 + 512); + EXPECT_NEAR(get_mag_strength_tesla(60, -5) * 1e9, 51158, 145 + 512); + EXPECT_NEAR(get_mag_strength_tesla(60, 0) * 1e9, 51192, 145 + 512); + EXPECT_NEAR(get_mag_strength_tesla(60, 5) * 1e9, 51300, 145 + 513); + EXPECT_NEAR(get_mag_strength_tesla(60, 10) * 1e9, 51480, 145 + 515); + EXPECT_NEAR(get_mag_strength_tesla(60, 15) * 1e9, 51731, 145 + 517); + EXPECT_NEAR(get_mag_strength_tesla(60, 20) * 1e9, 52054, 145 + 521); + EXPECT_NEAR(get_mag_strength_tesla(60, 25) * 1e9, 52452, 145 + 525); + EXPECT_NEAR(get_mag_strength_tesla(60, 30) * 1e9, 52927, 145 + 529); + EXPECT_NEAR(get_mag_strength_tesla(60, 35) * 1e9, 53482, 145 + 535); + EXPECT_NEAR(get_mag_strength_tesla(60, 40) * 1e9, 54115, 145 + 541); + EXPECT_NEAR(get_mag_strength_tesla(60, 45) * 1e9, 54822, 145 + 548); + EXPECT_NEAR(get_mag_strength_tesla(60, 50) * 1e9, 55592, 145 + 556); + EXPECT_NEAR(get_mag_strength_tesla(60, 55) * 1e9, 56411, 145 + 564); + EXPECT_NEAR(get_mag_strength_tesla(60, 60) * 1e9, 57259, 145 + 573); + EXPECT_NEAR(get_mag_strength_tesla(60, 65) * 1e9, 58110, 145 + 581); + EXPECT_NEAR(get_mag_strength_tesla(60, 70) * 1e9, 58937, 145 + 589); + EXPECT_NEAR(get_mag_strength_tesla(60, 75) * 1e9, 59709, 145 + 597); + EXPECT_NEAR(get_mag_strength_tesla(60, 80) * 1e9, 60397, 145 + 604); + EXPECT_NEAR(get_mag_strength_tesla(60, 85) * 1e9, 60974, 145 + 610); + EXPECT_NEAR(get_mag_strength_tesla(60, 90) * 1e9, 61417, 145 + 614); + EXPECT_NEAR(get_mag_strength_tesla(60, 95) * 1e9, 61707, 145 + 617); + EXPECT_NEAR(get_mag_strength_tesla(60, 100) * 1e9, 61834, 145 + 618); + EXPECT_NEAR(get_mag_strength_tesla(60, 105) * 1e9, 61796, 145 + 618); + EXPECT_NEAR(get_mag_strength_tesla(60, 110) * 1e9, 61596, 145 + 616); + EXPECT_NEAR(get_mag_strength_tesla(60, 115) * 1e9, 61246, 145 + 612); + EXPECT_NEAR(get_mag_strength_tesla(60, 120) * 1e9, 60766, 145 + 608); + EXPECT_NEAR(get_mag_strength_tesla(60, 125) * 1e9, 60179, 145 + 602); + EXPECT_NEAR(get_mag_strength_tesla(60, 130) * 1e9, 59515, 145 + 595); + EXPECT_NEAR(get_mag_strength_tesla(60, 135) * 1e9, 58802, 145 + 588); + EXPECT_NEAR(get_mag_strength_tesla(60, 140) * 1e9, 58070, 145 + 581); + EXPECT_NEAR(get_mag_strength_tesla(60, 145) * 1e9, 57346, 145 + 573); + EXPECT_NEAR(get_mag_strength_tesla(60, 150) * 1e9, 56653, 145 + 567); + EXPECT_NEAR(get_mag_strength_tesla(60, 155) * 1e9, 56009, 145 + 560); + EXPECT_NEAR(get_mag_strength_tesla(60, 160) * 1e9, 55428, 145 + 554); + EXPECT_NEAR(get_mag_strength_tesla(60, 165) * 1e9, 54921, 145 + 549); + EXPECT_NEAR(get_mag_strength_tesla(60, 170) * 1e9, 54497, 145 + 545); + EXPECT_NEAR(get_mag_strength_tesla(60, 175) * 1e9, 54164, 145 + 542); EXPECT_NEAR(get_mag_strength_tesla(60, 180) * 1e9, 53927, 145 + 539); } diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index e7ea1a21009d..444f5b0c8dba 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -598,6 +598,9 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_ if (!_health_and_arming_checks.canArm(_vehicle_status.nav_state)) { tune_negative(true); + mavlink_log_critical(&_mavlink_log_pub, "Arming denied: Resolve system health failures first\t"); + events::send(events::ID("commander_arm_denied_resolve_failures"), {events::Log::Critical, events::LogInternal::Info}, + "Arming denied: Resolve system health failures first"); return TRANSITION_DENIED; } } @@ -1201,7 +1204,7 @@ Commander::handle_command(const vehicle_command_s &cmd) #if defined(CONFIG_BOARDCTL_RESET) - } else if ((param1 == 1) && !isArmed() && (px4_reboot_request(false, 400_ms) == 0)) { + } else if ((param1 == 1) && !isArmed() && (px4_reboot_request(REBOOT_REQUEST, 400_ms) == 0)) { // 1: Reboot autopilot answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); @@ -1221,7 +1224,7 @@ Commander::handle_command(const vehicle_command_s &cmd) #if defined(CONFIG_BOARDCTL_RESET) - } else if ((param1 == 3) && !isArmed() && (px4_reboot_request(true, 400_ms) == 0)) { + } else if ((param1 == 3) && !isArmed() && (px4_reboot_request(REBOOT_TO_BOOTLOADER, 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); @@ -1441,6 +1444,10 @@ Commander::handle_command(const vehicle_command_s &cmd) answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); break; + case vehicle_command_s::VEHICLE_CMD_DO_SET_ACTUATOR: + answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); + break; + case vehicle_command_s::VEHICLE_CMD_START_RX_PAIR: case vehicle_command_s::VEHICLE_CMD_CUSTOM_0: case vehicle_command_s::VEHICLE_CMD_CUSTOM_1: @@ -1477,7 +1484,6 @@ Commander::handle_command(const vehicle_command_s &cmd) case vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW: case vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE: case vehicle_command_s::VEHICLE_CMD_CONFIGURE_ACTUATOR: - case vehicle_command_s::VEHICLE_CMD_DO_SET_ACTUATOR: case vehicle_command_s::VEHICLE_CMD_REQUEST_MESSAGE: case vehicle_command_s::VEHICLE_CMD_DO_WINCH: case vehicle_command_s::VEHICLE_CMD_DO_GRIPPER: @@ -2416,7 +2422,14 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning) if (_cpuload_sub.copy(&cpuload)) { const float cpuload_percent = cpuload.load * 100.f; - bool overload = (cpuload_percent > _param_com_cpu_max.get()) || (cpuload.ram_usage > 0.99f); + bool overload = false; + + // Only check CPU load if it hasn't been disabled + if (!(_param_com_cpu_max.get() < FLT_EPSILON)) { + overload = (cpuload_percent > _param_com_cpu_max.get()); + } + + overload = overload || (cpuload.ram_usage > 0.99f); if (_overload_start == 0 && overload) { _overload_start = time_now_us; diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecksTest.cpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecksTest.cpp index bc919080bf56..912d3bb1293b 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecksTest.cpp +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecksTest.cpp @@ -55,7 +55,7 @@ class ReporterTest : public ::testing::Test void SetUp() override { // ensure topic exists, otherwise we might lose first queued events - orb_advertise_queue(ORB_ID(event), nullptr, event_s::ORB_QUEUE_LENGTH); + orb_advertise(ORB_ID(event), nullptr); } }; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp index 460ed302ed7d..f13977199e7a 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp @@ -285,12 +285,16 @@ void BatteryChecks::rtlEstimateCheck(const Context &context, Report &reporter, f rtl_time_estimate_s rtl_time_estimate; // Compare estimate of RTL time to estimate of remaining flight time + // add hysterisis: if already in the condition, only get out of it if the remaining flight time is significantly higher again + const float hysteresis_factor = reporter.failsafeFlags().battery_low_remaining_time ? 1.1f : 1.0f; + reporter.failsafeFlags().battery_low_remaining_time = _rtl_time_estimate_sub.copy(&rtl_time_estimate) - && (hrt_absolute_time() - rtl_time_estimate.timestamp) < 2_s + && (hrt_absolute_time() - rtl_time_estimate.timestamp) < 3_s && rtl_time_estimate.valid && context.isArmed() && PX4_ISFINITE(worst_battery_time_s) - && rtl_time_estimate.safe_time_estimate >= worst_battery_time_s; + && rtl_time_estimate.safe_time_estimate * hysteresis_factor >= worst_battery_time_s; + if (reporter.failsafeFlags().battery_low_remaining_time) { /* EVENT diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp index 9bb33f2bd24f..1cdd097b12aa 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp @@ -293,7 +293,9 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor mavlink_log_warning(reporter.mavlink_log_pub(), "GNSS data fusion stopped\t"); } - events::send(events::ID("check_estimator_gnss_fusion_stopped"), {events::Log::Error, events::LogInternal::Info}, + // only report this failure as critical if not already in a local position invalid state + events::Log log_level = reporter.failsafeFlags().local_position_invalid ? events::Log::Info : events::Log::Error; + events::send(events::ID("check_estimator_gnss_fusion_stopped"), {log_level, events::LogInternal::Info}, "GNSS data fusion stopped"); } else if (!_gps_was_fused && ekf_gps_fusion) { diff --git a/src/modules/commander/HealthAndArmingChecks/checks/flightTimeCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/flightTimeCheck.cpp index d5e122935d1a..adb8a02d7e00 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/flightTimeCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/flightTimeCheck.cpp @@ -38,6 +38,11 @@ void FlightTimeChecks::checkAndReport(const Context &context, Report &reporter) if (_param_com_flt_time_max.get() > FLT_EPSILON && context.status().takeoff_time != 0 && (hrt_absolute_time() - context.status().takeoff_time) > (1_s * _param_com_flt_time_max.get())) { reporter.failsafeFlags().flight_time_limit_exceeded = true; + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Maximum flight time reached\t"); + } + /* EVENT * @description * @@ -48,10 +53,6 @@ void FlightTimeChecks::checkAndReport(const Context &context, Report &reporter) events::ID("check_flight_time_limit"), events::Log::Error, "Maximum flight time reached"); - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Maximum flight time reached\t"); - } - } else { reporter.failsafeFlags().flight_time_limit_exceeded = false; } diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index f642541fb74b..48c93982495a 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -153,7 +153,7 @@ int buzzer_init() tune_durations[tune_control_s::TUNE_ID_BATTERY_WARNING_SLOW] = 800000; tune_durations[tune_control_s::TUNE_ID_SINGLE_BEEP] = 300000; - tune_control_pub = orb_advertise_queue(ORB_ID(tune_control), &tune_control, tune_control_s::ORB_QUEUE_LENGTH); + tune_control_pub = orb_advertise(ORB_ID(tune_control), &tune_control); return PX4_OK; } @@ -330,7 +330,7 @@ int led_init() led_control.mode = led_control_s::MODE_OFF; led_control.priority = 0; led_control.timestamp = hrt_absolute_time(); - led_control_pub = orb_advertise_queue(ORB_ID(led_control), &led_control, led_control_s::ORB_QUEUE_LENGTH); + led_control_pub = orb_advertise(ORB_ID(led_control), &led_control); /* first open normal LEDs */ fd_leds = px4_open(LED0_DEVICE_PATH, O_RDWR); diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index e01b45276d5b..8f2c3d51ced0 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -1030,3 +1030,17 @@ PARAM_DEFINE_INT32(COM_THROW_EN, 0); * @unit m/s */ PARAM_DEFINE_FLOAT(COM_THROW_SPEED, 5); + +/** + * Remaining flight time low failsafe + * + * Action the system takes when the remaining flight time is below + * the estimated time it takes to reach the RTL destination. + * + * @group Commander + * @value 0 None + * @value 1 Warning + * @value 3 Return + * @increment 1 + */ +PARAM_DEFINE_INT32(COM_FLTT_LOW_ACT, 3); diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index 61cbff2beb39..9a9b1e7cad2c 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -365,6 +365,36 @@ FailsafeBase::ActionOptions Failsafe::fromHighWindLimitActParam(int param_value) return options; } +FailsafeBase::ActionOptions Failsafe::fromRemainingFlightTimeLowActParam(int param_value) +{ + ActionOptions options{}; + + options.allow_user_takeover = UserTakeoverAllowed::Auto; + options.cause = Cause::RemainingFlightTimeLow; + + switch (command_after_remaining_flight_time_low(param_value)) { + case command_after_remaining_flight_time_low::None: + options.action = Action::None; + break; + + case command_after_remaining_flight_time_low::Warning: + options.action = Action::Warn; + break; + + case command_after_remaining_flight_time_low::Return_mode: + options.action = Action::RTL; + options.clear_condition = ClearCondition::OnModeChangeOrDisarm; + break; + + default: + options.action = Action::None; + break; + + } + + return options; +} + void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, const failsafe_flags_s &status_flags) { @@ -444,9 +474,10 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, CHECK_FAILSAFE(status_flags, geofence_breached, fromGfActParam(_param_gf_action.get()).cannotBeDeferred()); - // Battery + // Battery flight time remaining failsafe + CHECK_FAILSAFE(status_flags, battery_low_remaining_time, - ActionOptions(Action::RTL).causedBy(Cause::BatteryLow).clearOn(ClearCondition::OnModeChangeOrDisarm)); + ActionOptions(fromRemainingFlightTimeLowActParam(_param_com_fltt_low_act.get()))); if ((_armed_time != 0) && (time_us < _armed_time + static_cast(_param_com_spoolup_time.get() * 1_s)) @@ -457,6 +488,7 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, CHECK_FAILSAFE(status_flags, battery_unhealthy, Action::Warn); } + // Battery low failsafe switch (status_flags.battery_warning) { case battery_status_s::BATTERY_WARNING_LOW: _last_state_battery_warning_low = checkFailsafe(_caller_id_battery_warning_low, _last_state_battery_warning_low, diff --git a/src/modules/commander/failsafe/failsafe.h b/src/modules/commander/failsafe/failsafe.h index 26981eb75499..bb81dd02baeb 100644 --- a/src/modules/commander/failsafe/failsafe.h +++ b/src/modules/commander/failsafe/failsafe.h @@ -141,6 +141,12 @@ class Failsafe : public FailsafeBase Land_mode = 5 }; + enum class command_after_remaining_flight_time_low : int32_t { + None = 0, + Warning = 1, + Return_mode = 3 + }; + static ActionOptions fromNavDllOrRclActParam(int param_value); static ActionOptions fromGfActParam(int param_value); @@ -150,6 +156,7 @@ class Failsafe : public FailsafeBase static ActionOptions fromQuadchuteActParam(int param_value); static Action fromOffboardLossActParam(int param_value, uint8_t &user_intended_mode); static ActionOptions fromHighWindLimitActParam(int param_value); + static ActionOptions fromRemainingFlightTimeLowActParam(int param_value); const int _caller_id_mode_fallback{genCallerId()}; bool _last_state_mode_fallback{false}; @@ -182,7 +189,8 @@ class Failsafe : public FailsafeBase (ParamInt) _param_com_low_bat_act, (ParamInt) _param_com_obl_rc_act, (ParamInt) _param_com_qc_act, - (ParamInt) _param_com_wind_max_act + (ParamInt) _param_com_wind_max_act, + (ParamInt) _param_com_fltt_low_act ); }; diff --git a/src/modules/commander/failsafe/framework.cpp b/src/modules/commander/failsafe/framework.cpp index 88123548a76d..16a3678d81b5 100644 --- a/src/modules/commander/failsafe/framework.cpp +++ b/src/modules/commander/failsafe/framework.cpp @@ -195,7 +195,7 @@ void FailsafeBase::notifyUser(uint8_t user_intended_mode, Action action, Action events::send( events::ID("commander_failsafe_enter_generic_hold"), {events::Log::Critical, events::LogInternal::Warning}, - "Failsafe activated, triggering {2} in {3} seconds", mavlink_mode, failsafe_action, + "Failsafe, triggering {2} in {3} seconds", mavlink_mode, failsafe_action, (uint16_t) delay_s); } else { @@ -204,7 +204,7 @@ void FailsafeBase::notifyUser(uint8_t user_intended_mode, Action action, Action events::send( events::ID("commander_failsafe_enter_hold"), {events::Log::Critical, events::LogInternal::Warning}, - "Failsafe activated due to {4}, triggering {2} in {3} seconds", mavlink_mode, failsafe_action, + "{4}, triggering {2} in {3} seconds", mavlink_mode, failsafe_action, (uint16_t) delay_s, failsafe_cause); } @@ -231,7 +231,7 @@ void FailsafeBase::notifyUser(uint8_t user_intended_mode, Action action, Action events::send( events::ID("commander_failsafe_enter_generic"), {events::Log::Critical, events::LogInternal::Warning}, - "Failsafe activated, triggering {2}", mavlink_mode, failsafe_action); + "Failsafe, triggering {2}", mavlink_mode, failsafe_action); } } else { @@ -250,6 +250,11 @@ void FailsafeBase::notifyUser(uint8_t user_intended_mode, Action action, Action events::send(events::ID("commander_failsafe_enter_crit_low_bat_warn"), {events::Log::Emergency, events::LogInternal::Info}, "Emergency battery level, land immediately"); + } else if (cause == Cause::RemainingFlightTimeLow) { + events::send(events::ID("commander_failsafe_enter_low_flight_time_warn"), + {events::Log::Warning, events::LogInternal::Info}, + "Low remaining flight time, return advised"); + } else { /* EVENT * @description No action is triggered. @@ -267,7 +272,7 @@ void FailsafeBase::notifyUser(uint8_t user_intended_mode, Action action, Action events::send( events::ID("commander_failsafe_enter"), {events::Log::Critical, events::LogInternal::Warning}, - "Failsafe activated due to {3}, triggering {2}", mavlink_mode, failsafe_action, failsafe_cause); + "{3}, triggering {2}", mavlink_mode, failsafe_action, failsafe_cause); } } @@ -512,6 +517,8 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s break; } + returned_state.cause = Cause::Generic; + // fallthrough case Action::FallbackAltCtrl: if (modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_ALTCTL)) { @@ -519,6 +526,8 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s break; } + returned_state.cause = Cause::Generic; + // fallthrough case Action::FallbackStab: if (modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_STAB)) { @@ -526,6 +535,8 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s break; } // else: fall through here as well. If stabilized isn't available, we most certainly end up in Terminate + returned_state.cause = Cause::Generic; + // fallthrough case Action::Hold: if (modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER)) { @@ -533,6 +544,8 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s break; } + returned_state.cause = Cause::Generic; + // fallthrough case Action::RTL: if (modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_AUTO_RTL)) { @@ -540,6 +553,8 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s break; } + returned_state.cause = Cause::Generic; + // fallthrough case Action::Land: if (modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_AUTO_LAND)) { @@ -547,6 +562,8 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s break; } + returned_state.cause = Cause::Generic; + // fallthrough case Action::Descend: if (modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_DESCEND)) { @@ -554,6 +571,8 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s break; } + returned_state.cause = Cause::Generic; + // fallthrough case Action::Terminate: selected_action = Action::Terminate; diff --git a/src/modules/commander/failsafe/framework.h b/src/modules/commander/failsafe/framework.h index 2d7b36ee7a59..9f989582f7ce 100644 --- a/src/modules/commander/failsafe/framework.h +++ b/src/modules/commander/failsafe/framework.h @@ -83,6 +83,7 @@ class FailsafeBase: public ModuleParams BatteryLow, BatteryCritical, BatteryEmergency, + RemainingFlightTimeLow, Count }; @@ -146,7 +147,7 @@ class FailsafeBase: public ModuleParams bool rc_sticks_takeover_request, const failsafe_flags_s &status_flags); - bool inFailsafe() const { return _selected_action != Action::None; } + bool inFailsafe() const { return (_selected_action != Action::None && _selected_action != Action::Warn); } Action selectedAction() const { return _selected_action; } diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 3fff697d4940..02ab015e0f17 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -1006,8 +1006,8 @@ int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radian return PX4_ERROR; } - calibration_log_critical(mavlink_log_pub, "Assuming vehicle is facing heading %.1f degrees", - (double)math::radians(heading_radians)); + calibration_log_info(mavlink_log_pub, "Assuming vehicle is facing heading %.1f degrees", + (double)math::degrees(heading_radians)); matrix::Eulerf euler{matrix::Quatf{attitude.q}}; euler(2) = heading_radians; diff --git a/src/modules/commander/worker_thread.cpp b/src/modules/commander/worker_thread.cpp index bb73c1e6ccb5..9859da498b3e 100644 --- a/src/modules/commander/worker_thread.cpp +++ b/src/modules/commander/worker_thread.cpp @@ -177,7 +177,7 @@ void WorkerThread::threadEntry() param_reset_specific(reset_cal, sizeof(reset_cal) / sizeof(reset_cal[0])); _ret_value = param_save_default(true); #if defined(CONFIG_BOARDCTL_RESET) - px4_reboot_request(false, 400_ms); + px4_reboot_request(REBOOT_REQUEST, 400_ms); #endif // CONFIG_BOARDCTL_RESET break; } diff --git a/src/modules/control_allocator/ControlAllocation/CMakeLists.txt b/src/modules/control_allocator/ControlAllocation/CMakeLists.txt index ecf5f0d2e447..4da638aac8cb 100644 --- a/src/modules/control_allocator/ControlAllocation/CMakeLists.txt +++ b/src/modules/control_allocator/ControlAllocation/CMakeLists.txt @@ -44,3 +44,4 @@ target_include_directories(ControlAllocation PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) target_link_libraries(ControlAllocation PRIVATE mathlib) px4_add_unit_gtest(SRC ControlAllocationPseudoInverseTest.cpp LINKLIBS ControlAllocation) +px4_add_functional_gtest(SRC ControlAllocationSequentialDesaturationTest.cpp LINKLIBS ControlAllocation ActuatorEffectiveness) diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp index 55b5a870fa09..c60784a03c2c 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp @@ -131,9 +131,9 @@ class ControlAllocation void setControlSetpoint(const matrix::Vector &control) { _control_sp = control; } /** - * Set the desired control vector + * Get the desired control vector * - * @param Control vector + * @return Control vector */ const matrix::Vector &getControlSetpoint() const { return _control_sp; } diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturationTest.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturationTest.cpp new file mode 100644 index 000000000000..60392330c939 --- /dev/null +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturationTest.cpp @@ -0,0 +1,385 @@ +/**************************************************************************** + * + * Copyright (C) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ControlAllocationSequentialDesaturationTest.cpp + * + * Tests for Control Allocation Sequential Desaturation Algorithms + * + */ + +#include +#include +#include <../ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp> + +using namespace matrix; + +namespace +{ + +// Makes and returns a Geometry object for a "standard" quad-x quadcopter. +ActuatorEffectivenessRotors::Geometry make_quad_x_geometry() +{ + ActuatorEffectivenessRotors::Geometry geometry = {}; + geometry.rotors[0].position(0) = 1.0f; + geometry.rotors[0].position(1) = 1.0f; + geometry.rotors[0].position(2) = 0.0f; + geometry.rotors[0].axis(0) = 0.0f; + geometry.rotors[0].axis(1) = 0.0f; + geometry.rotors[0].axis(2) = -1.0f; + geometry.rotors[0].thrust_coef = 1.0f; + geometry.rotors[0].moment_ratio = 0.05f; + + geometry.rotors[1].position(0) = -1.0f; + geometry.rotors[1].position(1) = -1.0f; + geometry.rotors[1].position(2) = 0.0f; + geometry.rotors[1].axis(0) = 0.0f; + geometry.rotors[1].axis(1) = 0.0f; + geometry.rotors[1].axis(2) = -1.0f; + geometry.rotors[1].thrust_coef = 1.0f; + geometry.rotors[1].moment_ratio = 0.05f; + + geometry.rotors[2].position(0) = 1.0f; + geometry.rotors[2].position(1) = -1.0f; + geometry.rotors[2].position(2) = 0.0f; + geometry.rotors[2].axis(0) = 0.0f; + geometry.rotors[2].axis(1) = 0.0f; + geometry.rotors[2].axis(2) = -1.0f; + geometry.rotors[2].thrust_coef = 1.0f; + geometry.rotors[2].moment_ratio = -0.05f; + + geometry.rotors[3].position(0) = -1.0f; + geometry.rotors[3].position(1) = 1.0f; + geometry.rotors[3].position(2) = 0.0f; + geometry.rotors[3].axis(0) = 0.0f; + geometry.rotors[3].axis(1) = 0.0f; + geometry.rotors[3].axis(2) = -1.0f; + geometry.rotors[3].thrust_coef = 1.0f; + geometry.rotors[3].moment_ratio = -0.05f; + + geometry.num_rotors = 4; + + return geometry; +} + +// Returns an effective matrix for a sample quad-copter configuration. +ActuatorEffectiveness::EffectivenessMatrix make_quad_x_effectiveness() +{ + ActuatorEffectiveness::EffectivenessMatrix effectiveness; + effectiveness.setZero(); + const auto geometry = make_quad_x_geometry(); + ActuatorEffectivenessRotors::computeEffectivenessMatrix(geometry, effectiveness); + return effectiveness; +} + +// Configures a ControlAllocationSequentialDesaturation object for a sample quad-copter. +void setup_quad_allocator(ControlAllocationSequentialDesaturation &allocator) +{ + const auto effectiveness = make_quad_x_effectiveness(); + matrix::Vector actuator_trim; + matrix::Vector linearization_point; + constexpr bool UPDATE_NORMALIZATION_SCALE{false}; + allocator.setEffectivenessMatrix( + effectiveness, + actuator_trim, + linearization_point, + ActuatorEffectiveness::NUM_ACTUATORS, + UPDATE_NORMALIZATION_SCALE + ); +} + +static constexpr float EXPECT_NEAR_TOL{1e-4f}; + +} // namespace + +// This tests that yaw-only control setpoint at zero actuator setpoint results in zero actuator +// allocation. +TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledOnlyYaw) +{ + ControlAllocationSequentialDesaturation allocator; + setup_quad_allocator(allocator); + matrix::Vector control_sp; + control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f; + control_sp(ControlAllocation::ControlAxis::PITCH) = 0.f; + control_sp(ControlAllocation::ControlAxis::YAW) = 1.f; + control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_Z) = 0.f; + allocator.setControlSetpoint(control_sp); + + // Since MC_AIRMODE was not set explicitly, assume airmode is disabled. + allocator.allocate(); + + const auto &actuator_sp = allocator.getActuatorSetpoint(); + matrix::Vector zero; + EXPECT_EQ(actuator_sp, zero); +} + +// This tests that a control setpoint for z-thrust returns the desired actuator setpoint. +// Each motor should have an actuator setpoint that when summed together should be equal to +// control setpoint. +TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledThrustZ) +{ + ControlAllocationSequentialDesaturation allocator; + setup_quad_allocator(allocator); + matrix::Vector control_sp; + // Negative, because +z is "downward". + constexpr float THRUST_Z_TOTAL{-0.75f}; + control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f; + control_sp(ControlAllocation::ControlAxis::PITCH) = 0.f; + control_sp(ControlAllocation::ControlAxis::YAW) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL; + allocator.setControlSetpoint(control_sp); + + // Since MC_AIRMODE was not set explicitly, assume airmode is disabled. + allocator.allocate(); + + const auto &actuator_sp = allocator.getActuatorSetpoint(); + constexpr int MOTOR_COUNT{4}; + constexpr float THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT}; + + for (int i{0}; i < MOTOR_COUNT; ++i) { + EXPECT_NEAR(actuator_sp(i), THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + } + + for (int i{MOTOR_COUNT}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) { + EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL); + } +} + +// This tests that a control setpoint for z-thrust + yaw returns the desired actuator setpoint. +// This test does not saturate the yaw response. +TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledThrustAndYaw) +{ + ControlAllocationSequentialDesaturation allocator; + setup_quad_allocator(allocator); + matrix::Vector control_sp; + // Negative, because +z is "downward". + constexpr float THRUST_Z_TOTAL{-0.75f}; + // This is low enough to not saturate the motors. + constexpr float YAW_CONTROL_SP{0.02f}; + control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f; + control_sp(ControlAllocation::ControlAxis::PITCH) = 0.f; + control_sp(ControlAllocation::ControlAxis::YAW) = YAW_CONTROL_SP; + control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL; + allocator.setControlSetpoint(control_sp); + + // Since MC_AIRMODE was not set explicitly, assume airmode is disabled. + allocator.allocate(); + + const auto &actuator_sp = allocator.getActuatorSetpoint(); + // This value is based off of the effectiveness matrix. If the effectiveness matrix is changed, + // this will need to be changed. + constexpr float YAW_EFFECTIVENESS_FACTOR{5.f}; + constexpr float YAW_DIFF_PER_MOTOR{YAW_CONTROL_SP * YAW_EFFECTIVENESS_FACTOR}; + // At yaw condition, there will be 2 different actuator values. + constexpr int MOTOR_COUNT{4}; + constexpr float HIGH_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT + YAW_DIFF_PER_MOTOR}; + constexpr float LOW_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT - YAW_DIFF_PER_MOTOR}; + + for (int i{0}; i < MOTOR_COUNT / 2; ++i) { + EXPECT_NEAR(actuator_sp(i), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + } + + for (int i{MOTOR_COUNT / 2}; i < MOTOR_COUNT; ++i) { + EXPECT_NEAR(actuator_sp(i), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + } + + for (int i{MOTOR_COUNT}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) { + EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL); + } +} + +// This tests that a control setpoint for z-thrust + yaw returns the desired actuator setpoint. +// This test saturates the yaw response, but does not reduce total thrust. +TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledThrustAndSaturatedYaw) +{ + ControlAllocationSequentialDesaturation allocator; + setup_quad_allocator(allocator); + matrix::Vector control_sp; + // Negative, because +z is "downward". + constexpr float THRUST_Z_TOTAL{-0.75f}; + // This is arbitrarily high to trigger strongest possible (saturated) yaw response. + constexpr float YAW_CONTROL_SP{0.25f}; + control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f; + control_sp(ControlAllocation::ControlAxis::PITCH) = 0.f; + control_sp(ControlAllocation::ControlAxis::YAW) = YAW_CONTROL_SP; + control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL; + allocator.setControlSetpoint(control_sp); + + // Since MC_AIRMODE was not set explicitly, assume airmode is disabled. + allocator.allocate(); + + const auto &actuator_sp = allocator.getActuatorSetpoint(); + // At max yaw, only 2 motors will carry all of the thrust. + constexpr int YAW_MOTORS{2}; + constexpr float THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / YAW_MOTORS}; + + for (int i{0}; i < YAW_MOTORS; ++i) { + EXPECT_NEAR(actuator_sp(i), THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + } + + for (int i{YAW_MOTORS}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) { + EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL); + } +} + +// This tests that a control setpoint for z-thrust + pitch returns the desired actuator setpoint. +// This test does not saturate the pitch response. +TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledThrustAndPitch) +{ + ControlAllocationSequentialDesaturation allocator; + setup_quad_allocator(allocator); + matrix::Vector control_sp; + // Negative, because +z is "downward". + constexpr float THRUST_Z_TOTAL{-0.75f}; + // This is low enough to not saturate the motors. + constexpr float PITCH_CONTROL_SP{0.1f}; + control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f; + control_sp(ControlAllocation::ControlAxis::PITCH) = PITCH_CONTROL_SP; + control_sp(ControlAllocation::ControlAxis::YAW) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL; + allocator.setControlSetpoint(control_sp); + + // Since MC_AIRMODE was not set explicitly, assume airmode is disabled. + allocator.allocate(); + + const auto &actuator_sp = allocator.getActuatorSetpoint(); + // This value is based off of the effectiveness matrix. If the effectiveness matrix is changed, + // this will need to be changed. + constexpr int MOTOR_COUNT{4}; + constexpr float PITCH_DIFF_PER_MOTOR{PITCH_CONTROL_SP / MOTOR_COUNT}; + // At control set point, there will be 2 different actuator values. + constexpr float HIGH_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT + PITCH_DIFF_PER_MOTOR}; + constexpr float LOW_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT - PITCH_DIFF_PER_MOTOR}; + EXPECT_NEAR(actuator_sp(0), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + EXPECT_NEAR(actuator_sp(1), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + EXPECT_NEAR(actuator_sp(2), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + EXPECT_NEAR(actuator_sp(3), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + + for (int i{MOTOR_COUNT}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) { + EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL); + } +} + +// This tests that a control setpoint for z-thrust + yaw returns the desired actuator setpoint. +// This test saturates yaw and demonstrates reduction of thrust for yaw. +TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledReducedThrustAndYaw) +{ + ControlAllocationSequentialDesaturation allocator; + setup_quad_allocator(allocator); + matrix::Vector control_sp; + // Negative, because +z is "downward". + constexpr float DESIRED_THRUST_Z_PER_MOTOR{0.8f}; + constexpr int MOTOR_COUNT{4}; + constexpr float THRUST_Z_TOTAL{-DESIRED_THRUST_Z_PER_MOTOR * MOTOR_COUNT}; + // This is arbitrarily high to trigger strongest possible (saturated) yaw response. + constexpr float YAW_CONTROL_SP{1.f}; + control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f; + control_sp(ControlAllocation::ControlAxis::PITCH) = 0.f; + control_sp(ControlAllocation::ControlAxis::YAW) = YAW_CONTROL_SP; + control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL; + allocator.setControlSetpoint(control_sp); + + // Since MC_AIRMODE was not set explicitly, assume airmode is disabled. + allocator.allocate(); + + const auto &actuator_sp = allocator.getActuatorSetpoint(); + // In the case of yaw saturation, thrust per motor will be reduced by the hard-coded + // magic-number yaw margin of 0.15f. + constexpr float YAW_MARGIN{0.15f}; // get this from a centralized source when available. + constexpr float YAW_DIFF_PER_MOTOR{1.0f + YAW_MARGIN - DESIRED_THRUST_Z_PER_MOTOR}; + // At control set point, there will be 2 different actuator values. + constexpr float HIGH_THRUST_Z_PER_MOTOR{DESIRED_THRUST_Z_PER_MOTOR + YAW_DIFF_PER_MOTOR - YAW_MARGIN}; + constexpr float LOW_THRUST_Z_PER_MOTOR{DESIRED_THRUST_Z_PER_MOTOR - YAW_DIFF_PER_MOTOR - YAW_MARGIN}; + EXPECT_NEAR(actuator_sp(0), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + EXPECT_NEAR(actuator_sp(1), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + EXPECT_NEAR(actuator_sp(2), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + EXPECT_NEAR(actuator_sp(3), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + + for (int i{MOTOR_COUNT}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) { + EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL); + } +} + +// This tests that a control setpoint for z-thrust + pitch returns the desired actuator setpoint. +// This test saturates the pitch response such that thrust is reduced to (partially) compensate. +TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledReducedThrustAndPitch) +{ + ControlAllocationSequentialDesaturation allocator; + setup_quad_allocator(allocator); + matrix::Vector control_sp; + // Negative, because +z is "downward". + constexpr float THRUST_Z_TOTAL{-0.75f * 4.f}; + // This is high enough to saturate the pitch control. + constexpr float PITCH_CONTROL_SP{2.f}; + control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f; + control_sp(ControlAllocation::ControlAxis::PITCH) = PITCH_CONTROL_SP; + control_sp(ControlAllocation::ControlAxis::YAW) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL; + allocator.setControlSetpoint(control_sp); + + // Since MC_AIRMODE was not set explicitly, assume airmode is disabled. + allocator.allocate(); + + const auto &actuator_sp = allocator.getActuatorSetpoint(); + constexpr int MOTOR_COUNT{4}; + // The maximum actuator value is + // THRUST_Z_TOTAL / MOTOR_COUNT + PITCH_CONTROL_SP / MOTOR_COUNT. + // The amount over 1 is the amount that each motor is reduced by. + // At control set point, there will be 2 different actuator values. + constexpr float OVERAGE_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT + PITCH_CONTROL_SP / MOTOR_COUNT - 1}; + EXPECT_TRUE(OVERAGE_PER_MOTOR > 0.f); + constexpr float HIGH_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT + PITCH_CONTROL_SP / MOTOR_COUNT - OVERAGE_PER_MOTOR}; + constexpr float LOW_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT - PITCH_CONTROL_SP / MOTOR_COUNT - OVERAGE_PER_MOTOR}; + EXPECT_NEAR(actuator_sp(0), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + EXPECT_NEAR(actuator_sp(1), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + EXPECT_NEAR(actuator_sp(2), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + EXPECT_NEAR(actuator_sp(3), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + + for (int i{MOTOR_COUNT}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) { + EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL); + } +} diff --git a/src/modules/dataman/dataman.cpp b/src/modules/dataman/dataman.cpp index 08ff29a4ee02..b1c1a057e14a 100644 --- a/src/modules/dataman/dataman.cpp +++ b/src/modules/dataman/dataman.cpp @@ -129,8 +129,12 @@ static unsigned g_func_counts[DM_NUMBER_OF_FUNCS]; /* 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_SAFE_POINTS_0] + DM_SECTOR_HDR_SIZE, + g_per_item_size[DM_KEY_SAFE_POINTS_1] + DM_SECTOR_HDR_SIZE, + g_per_item_size[DM_KEY_SAFE_POINTS_STATE] + DM_SECTOR_HDR_SIZE, + g_per_item_size[DM_KEY_FENCE_POINTS_0] + DM_SECTOR_HDR_SIZE, + g_per_item_size[DM_KEY_FENCE_POINTS_1] + DM_SECTOR_HDR_SIZE, + g_per_item_size[DM_KEY_FENCE_POINTS_STATE] + 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, @@ -445,9 +449,6 @@ static int _ram_clear(dm_item_t item) return -1; } - int i; - int result = 0; - /* Get the offset of 1st item of this type */ int offset = calculate_offset(item, 0); @@ -456,8 +457,10 @@ static int _ram_clear(dm_item_t item) return -1; } + int result = 0; + /* Clear all items of this type */ - for (i = 0; (unsigned)i < g_per_item_max_index[item]; i++) { + for (int i = 0; (unsigned)i < g_per_item_max_index[item]; i++) { uint8_t *buf = &dm_operations_data.ram.data[offset]; if (buf > dm_operations_data.ram.data_end) { @@ -479,8 +482,6 @@ _file_clear(dm_item_t item) return -1; } - int i, result = 0; - /* Get the offset of 1st item of this type */ int offset = calculate_offset(item, 0); @@ -489,8 +490,10 @@ _file_clear(dm_item_t item) return -1; } + int result = 0; + /* Clear all items of this type */ - for (i = 0; (unsigned)i < g_per_item_max_index[item]; i++) { + for (int i = 0; (unsigned)i < g_per_item_max_index[item]; i++) { char buf[1]; if (lseek(dm_operations_data.file.fd, offset, SEEK_SET) != offset) { @@ -565,13 +568,13 @@ _file_initialize(unsigned max_offset) PX4_ERR("Failed writing compat: %d", ret); } - for (uint32_t item = DM_KEY_SAFE_POINTS; item <= DM_KEY_MISSION_STATE; ++item) { + for (uint32_t item = DM_KEY_SAFE_POINTS_0; 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.mission_dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0; mission.count = 0; mission.current_seq = 0; mission.mission_id = 0u; @@ -583,8 +586,8 @@ _file_initialize(unsigned max_offset) stats.opaque_id = 0; 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)); + g_dm_ops->write(DM_KEY_FENCE_POINTS_STATE, 0, reinterpret_cast(&stats), sizeof(mission_stats_entry_s)); + g_dm_ops->write(DM_KEY_SAFE_POINTS_STATE, 0, reinterpret_cast(&stats), sizeof(mission_stats_entry_s)); } dm_operations_data.running = true; @@ -820,8 +823,6 @@ task_main(int argc, char *argv[]) static int start() { - int task; - px4_sem_init(&g_init_sema, 1, 0); /* g_init_sema use case is a signal */ @@ -829,9 +830,9 @@ start() px4_sem_setprotocol(&g_init_sema, SEM_PRIO_NONE); /* start the worker thread with low priority for disk IO */ - if ((task = px4_task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT - 10, - PX4_STACK_ADJUSTED(TASK_STACK_SIZE), task_main, - nullptr)) < 0) { + if (px4_task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT - 10, + PX4_STACK_ADJUSTED(TASK_STACK_SIZE), task_main, + nullptr) < 0) { px4_sem_destroy(&g_init_sema); PX4_ERR("task start failed"); return -1; @@ -880,10 +881,6 @@ Each type has a specific type and a fixed maximum amount of storage items, so th ### Implementation 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). - )DESCR_STR"); PRINT_MODULE_USAGE_NAME("dataman", "system"); diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index 49fd036aecb1..c8cdb05af061 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -44,8 +44,12 @@ /** 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_SAFE_POINTS_0 = 0, ///< Safe points storage 0 + DM_KEY_SAFE_POINTS_1, ///< Safe points storage 1 (alternate between 0 and 1) + DM_KEY_SAFE_POINTS_STATE, ///< Persistent safe point state + DM_KEY_FENCE_POINTS_0, ///< Fence vertex storage 0 + DM_KEY_FENCE_POINTS_1, ///< Fence vertex storage 1 (alternate between 0 and 1) + DM_KEY_FENCE_POINTS_STATE, ///< Persistent fence vertex state 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 @@ -66,7 +70,9 @@ typedef enum { #if defined(MEMORY_CONSTRAINED_SYSTEM) enum { DM_KEY_SAFE_POINTS_MAX = 8, + DM_KEY_SAFE_POINTS_STATE_MAX = 1, DM_KEY_FENCE_POINTS_MAX = 16, + DM_KEY_FENCE_POINTS_STATE_MAX = 1, DM_KEY_WAYPOINTS_OFFBOARD_0_MAX = NUM_MISSIONS_SUPPORTED, DM_KEY_WAYPOINTS_OFFBOARD_1_MAX = NUM_MISSIONS_SUPPORTED, DM_KEY_MISSION_STATE_MAX = 1, @@ -75,7 +81,9 @@ enum { #else enum { DM_KEY_SAFE_POINTS_MAX = 32, + DM_KEY_SAFE_POINTS_STATE_MAX = 1, DM_KEY_FENCE_POINTS_MAX = 64, + DM_KEY_FENCE_POINTS_STATE_MAX = 1, DM_KEY_WAYPOINTS_OFFBOARD_0_MAX = NUM_MISSIONS_SUPPORTED, DM_KEY_WAYPOINTS_OFFBOARD_1_MAX = NUM_MISSIONS_SUPPORTED, DM_KEY_MISSION_STATE_MAX = 1, @@ -86,35 +94,45 @@ enum { /* 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_SAFE_POINTS_MAX, + DM_KEY_SAFE_POINTS_STATE_MAX, + DM_KEY_FENCE_POINTS_MAX, DM_KEY_FENCE_POINTS_MAX, + DM_KEY_FENCE_POINTS_STATE_MAX, DM_KEY_WAYPOINTS_OFFBOARD_0_MAX, DM_KEY_WAYPOINTS_OFFBOARD_1_MAX, DM_KEY_MISSION_STATE_MAX, DM_KEY_COMPAT_MAX }; +struct dataman_compat_s { + uint64_t key; +}; + constexpr uint32_t MISSION_SAFE_POINT_SIZE = sizeof(struct mission_item_s); +constexpr uint32_t MISSION_SAFE_POINT_STATE_SIZE = sizeof(struct mission_stats_entry_s); constexpr uint32_t MISSION_FENCE_POINT_SIZE = sizeof(struct mission_fence_point_s); +constexpr uint32_t MISSION_FENCE_POINT_STATE_SIZE = sizeof(struct mission_stats_entry_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); +constexpr uint32_t DATAMAN_COMPAT_SIZE = sizeof(struct dataman_compat_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_SAFE_POINT_SIZE, + MISSION_SAFE_POINT_STATE_SIZE, + MISSION_FENCE_POINT_SIZE, MISSION_FENCE_POINT_SIZE, + MISSION_FENCE_POINT_STATE_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 4ULL +#define DM_COMPAT_VERSION 5ULL #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) + \ diff --git a/src/modules/differential_drive/CMakeLists.txt b/src/modules/differential_drive/CMakeLists.txt new file mode 100644 index 000000000000..2e0e3583b9af --- /dev/null +++ b/src/modules/differential_drive/CMakeLists.txt @@ -0,0 +1,52 @@ +############################################################################ +# +# Copyright (c) 2023-2024 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +add_subdirectory(DifferentialDriveControl) +add_subdirectory(DifferentialDriveGuidance) +add_subdirectory(DifferentialDriveKinematics) + +px4_add_module( + MODULE modules__differential_drive + MAIN differential_drive + SRCS + DifferentialDrive.cpp + DifferentialDrive.hpp + DEPENDS + DifferentialDriveControl + DifferentialDriveGuidance + DifferentialDriveKinematics + px4_work_queue + modules__control_allocator # for parameter CA_R_REV + MODULE_CONFIG + module.yaml +) diff --git a/src/modules/differential_drive_control/DifferentialDriveControl.cpp b/src/modules/differential_drive/DifferentialDrive.cpp similarity index 55% rename from src/modules/differential_drive_control/DifferentialDriveControl.cpp rename to src/modules/differential_drive/DifferentialDrive.cpp index 41e509349931..70553ffeb369 100644 --- a/src/modules/differential_drive_control/DifferentialDriveControl.cpp +++ b/src/modules/differential_drive/DifferentialDrive.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * Copyright (c) 2023-2024 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -31,38 +31,37 @@ * ****************************************************************************/ -#include "DifferentialDriveControl.hpp" +#include "DifferentialDrive.hpp" -using namespace time_literals; -using namespace matrix; -namespace differential_drive_control -{ - -DifferentialDriveControl::DifferentialDriveControl() : +DifferentialDrive::DifferentialDrive() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) { updateParams(); } -bool DifferentialDriveControl::init() +bool DifferentialDrive::init() { ScheduleOnInterval(10_ms); // 100 Hz return true; } -void DifferentialDriveControl::updateParams() +void DifferentialDrive::updateParams() { ModuleParams::updateParams(); - _max_speed = _param_rdd_max_wheel_speed.get() * _param_rdd_wheel_radius.get(); - _max_angular_velocity = _max_speed / (_param_rdd_wheel_base.get() / 2.f); _differential_drive_kinematics.setWheelBase(_param_rdd_wheel_base.get()); + + _max_speed = _param_rdd_wheel_speed.get() * _param_rdd_wheel_radius.get(); + _differential_drive_guidance.setMaxSpeed(_max_speed); _differential_drive_kinematics.setMaxSpeed(_max_speed); + + _max_angular_velocity = _max_speed / (_param_rdd_wheel_base.get() / 2.f); + _differential_drive_guidance.setMaxAngularVelocity(_max_angular_velocity); _differential_drive_kinematics.setMaxAngularVelocity(_max_angular_velocity); } -void DifferentialDriveControl::Run() +void DifferentialDrive::Run() { if (should_exit()) { ScheduleClear(); @@ -70,20 +69,32 @@ void DifferentialDriveControl::Run() } hrt_abstime now = hrt_absolute_time(); + const float dt = math::min((now - _time_stamp_last), 5000_ms) / 1e6f; + _time_stamp_last = now; if (_parameter_update_sub.updated()) { - parameter_update_s pupdate; - _parameter_update_sub.copy(&pupdate); - + parameter_update_s parameter_update; + _parameter_update_sub.copy(¶meter_update); updateParams(); } if (_vehicle_control_mode_sub.updated()) { - vehicle_control_mode_s vehicle_control_mode; + vehicle_control_mode_s vehicle_control_mode{}; if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) { - _armed = vehicle_control_mode.flag_armed; - _manual_driving = vehicle_control_mode.flag_control_manual_enabled; // change this when more modes are supported + _manual_driving = vehicle_control_mode.flag_control_manual_enabled; + _mission_driving = vehicle_control_mode.flag_control_auto_enabled; + } + } + + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status{}; + + if (_vehicle_status_sub.copy(&vehicle_status)) { + const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + const bool spooled_up = armed && (hrt_elapsed_time(&vehicle_status.armed_time) > _param_com_spoolup_time.get() * 1_s); + _differential_drive_kinematics.setArmed(spooled_up); + _acro_driving = (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO); } } @@ -94,43 +105,42 @@ void DifferentialDriveControl::Run() manual_control_setpoint_s manual_control_setpoint{}; if (_manual_control_setpoint_sub.copy(&manual_control_setpoint)) { - _differential_drive_setpoint.timestamp = now; - _differential_drive_setpoint.speed = manual_control_setpoint.throttle * _param_rdd_speed_scale.get() * _max_speed; - _differential_drive_setpoint.yaw_rate = manual_control_setpoint.roll * _param_rdd_ang_velocity_scale.get() * - _max_angular_velocity; - _differential_drive_setpoint_pub.publish(_differential_drive_setpoint); + differential_drive_setpoint_s setpoint{}; + setpoint.speed = manual_control_setpoint.throttle * math::max(0.f, _param_rdd_speed_scale.get()) * _max_speed; + setpoint.yaw_rate = manual_control_setpoint.roll * _param_rdd_ang_velocity_scale.get() * _max_angular_velocity; + + // if acro mode, we activate the yaw rate control + if (_acro_driving) { + setpoint.closed_loop_speed_control = false; + setpoint.closed_loop_yaw_rate_control = true; + + } else { + setpoint.closed_loop_speed_control = false; + setpoint.closed_loop_yaw_rate_control = false; + } + + setpoint.timestamp = now; + _differential_drive_setpoint_pub.publish(setpoint); } } - } - - _differential_drive_setpoint_sub.update(&_differential_drive_setpoint); - // publish data to actuator_motors (output module) - // get the wheel speeds from the inverse kinematics class (DifferentialDriveKinematics) - Vector2f wheel_speeds = _differential_drive_kinematics.computeInverseKinematics( - _differential_drive_setpoint.speed, - _differential_drive_setpoint.yaw_rate); - - // Check if max_angular_wheel_speed is zero - const bool setpoint_timeout = (_differential_drive_setpoint.timestamp + 100_ms) < now; - const bool valid_max_speed = _param_rdd_speed_scale.get() > FLT_EPSILON; - - if (!_armed || setpoint_timeout || !valid_max_speed) { - wheel_speeds = {}; // stop + } else if (_mission_driving) { + // Mission mode + // directly receive setpoints from the guidance library + _differential_drive_guidance.computeGuidance( + _differential_drive_control.getVehicleYaw(), + _differential_drive_control.getVehicleBodyYawRate(), + dt + ); } - wheel_speeds = matrix::constrain(wheel_speeds, -1.f, 1.f); - - actuator_motors_s actuator_motors{}; - actuator_motors.reversible_flags = _param_r_rev.get(); // should be 3 see rc.rover_differential_defaults - wheel_speeds.copyTo(actuator_motors.control); - actuator_motors.timestamp = now; - _actuator_motors_pub.publish(actuator_motors); + _differential_drive_control.control(dt); + _differential_drive_kinematics.allocate(); } -int DifferentialDriveControl::task_spawn(int argc, char *argv[]) +int DifferentialDrive::task_spawn(int argc, char *argv[]) { - DifferentialDriveControl *instance = new DifferentialDriveControl(); + DifferentialDrive *instance = new DifferentialDrive(); if (instance) { _object.store(instance); @@ -151,12 +161,12 @@ int DifferentialDriveControl::task_spawn(int argc, char *argv[]) return PX4_ERROR; } -int DifferentialDriveControl::custom_command(int argc, char *argv[]) +int DifferentialDrive::custom_command(int argc, char *argv[]) { return print_usage("unknown command"); } -int DifferentialDriveControl::print_usage(const char *reason) +int DifferentialDrive::print_usage(const char *reason) { if (reason) { PX4_ERR("%s\n", reason); @@ -168,15 +178,13 @@ int DifferentialDriveControl::print_usage(const char *reason) Rover Differential Drive controller. )DESCR_STR"); - PRINT_MODULE_USAGE_NAME("differential_drive_control", "controller"); + PRINT_MODULE_USAGE_NAME("differential_drive", "controller"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; } -extern "C" __EXPORT int differential_drive_control_main(int argc, char *argv[]) +extern "C" __EXPORT int differential_drive_main(int argc, char *argv[]) { - return DifferentialDriveControl::main(argc, argv); + return DifferentialDrive::main(argc, argv); } - -} // namespace differential_drive_control diff --git a/src/modules/differential_drive_control/DifferentialDriveControl.hpp b/src/modules/differential_drive/DifferentialDrive.hpp similarity index 75% rename from src/modules/differential_drive_control/DifferentialDriveControl.hpp rename to src/modules/differential_drive/DifferentialDrive.hpp index 47133c8d2715..255bb0c52607 100644 --- a/src/modules/differential_drive_control/DifferentialDriveControl.hpp +++ b/src/modules/differential_drive/DifferentialDrive.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * Copyright (c) 2023-2024 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,41 +33,31 @@ #pragma once -// PX4 includes #include #include #include #include #include - -// uORB includes -#include -#include -#include -#include -#include -#include -#include #include #include -#include #include +#include +#include +#include +#include -// Standard library includes -#include - -// Local includes -#include +#include "DifferentialDriveControl/DifferentialDriveControl.hpp" +#include "DifferentialDriveGuidance/DifferentialDriveGuidance.hpp" +#include "DifferentialDriveKinematics/DifferentialDriveKinematics.hpp" -namespace differential_drive_control -{ +using namespace time_literals; -class DifferentialDriveControl : public ModuleBase, public ModuleParams, +class DifferentialDrive : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: - DifferentialDriveControl(); - ~DifferentialDriveControl() override = default; + DifferentialDrive(); + ~DifferentialDrive() override = default; /** @see ModuleBase */ static int task_spawn(int argc, char *argv[]); @@ -85,32 +75,31 @@ class DifferentialDriveControl : public ModuleBase, pu private: void Run() override; - - uORB::Subscription _differential_drive_setpoint_sub{ORB_ID(differential_drive_setpoint)}; uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; - uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; - uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; uORB::Publication _differential_drive_setpoint_pub{ORB_ID(differential_drive_setpoint)}; - differential_drive_setpoint_s _differential_drive_setpoint{}; - DifferentialDriveKinematics _differential_drive_kinematics{}; - - bool _armed = false; bool _manual_driving = false; + bool _mission_driving = false; + bool _acro_driving = false; + hrt_abstime _time_stamp_last{0}; /**< time stamp when task was last updated */ + + DifferentialDriveGuidance _differential_drive_guidance{this}; + DifferentialDriveControl _differential_drive_control{this}; + DifferentialDriveKinematics _differential_drive_kinematics{this}; + float _max_speed{0.f}; float _max_angular_velocity{0.f}; DEFINE_PARAMETERS( - (ParamFloat) _param_rdd_speed_scale, (ParamFloat) _param_rdd_ang_velocity_scale, - (ParamFloat) _param_rdd_max_wheel_speed, + (ParamFloat) _param_rdd_speed_scale, (ParamFloat) _param_rdd_wheel_base, + (ParamFloat) _param_rdd_wheel_speed, (ParamFloat) _param_rdd_wheel_radius, - (ParamInt) _param_r_rev + (ParamFloat) _param_com_spoolup_time ) }; - -} // namespace differential_drive_control diff --git a/src/modules/differential_drive/DifferentialDriveControl/CMakeLists.txt b/src/modules/differential_drive/DifferentialDriveControl/CMakeLists.txt new file mode 100644 index 000000000000..4610fa6c3d4d --- /dev/null +++ b/src/modules/differential_drive/DifferentialDriveControl/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2024 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(DifferentialDriveControl + DifferentialDriveControl.cpp +) + +target_link_libraries(DifferentialDriveControl PUBLIC pid) +target_include_directories(DifferentialDriveControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.cpp b/src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.cpp new file mode 100644 index 000000000000..728c5e666a10 --- /dev/null +++ b/src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.cpp @@ -0,0 +1,109 @@ +/**************************************************************************** + * + * Copyright (c) 2023-2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "DifferentialDriveControl.hpp" + +using namespace matrix; + +DifferentialDriveControl::DifferentialDriveControl(ModuleParams *parent) : ModuleParams(parent) +{ + pid_init(&_pid_angular_velocity, PID_MODE_DERIVATIV_NONE, 0.001f); + pid_init(&_pid_speed, PID_MODE_DERIVATIV_NONE, 0.001f); +} + +void DifferentialDriveControl::updateParams() +{ + ModuleParams::updateParams(); + + pid_set_parameters(&_pid_angular_velocity, + _param_rdd_p_gain_angular_velocity.get(), // Proportional gain + _param_rdd_i_gain_angular_velocity.get(), // Integral gain + 0, // Derivative gain + 20, // Integral limit + 200); // Output limit + + pid_set_parameters(&_pid_speed, + _param_rdd_p_gain_speed.get(), // Proportional gain + _param_rdd_i_gain_speed.get(), // Integral gain + 0, // Derivative gain + 2, // Integral limit + 200); // Output limit +} + +void DifferentialDriveControl::control(float dt) +{ + if (_vehicle_angular_velocity_sub.updated()) { + vehicle_angular_velocity_s vehicle_angular_velocity{}; + + if (_vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity)) { + _vehicle_body_yaw_rate = vehicle_angular_velocity.xyz[2]; + } + } + + if (_vehicle_attitude_sub.updated()) { + vehicle_attitude_s vehicle_attitude{}; + + if (_vehicle_attitude_sub.copy(&vehicle_attitude)) { + _vehicle_attitude_quaternion = Quatf(vehicle_attitude.q); + _vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi(); + } + } + + if (_vehicle_local_position_sub.updated()) { + vehicle_local_position_s vehicle_local_position{}; + + if (_vehicle_local_position_sub.copy(&vehicle_local_position)) { + Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); + Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame); + _vehicle_forward_speed = velocity_in_body_frame(0); + } + } + + _differential_drive_setpoint_sub.update(&_differential_drive_setpoint); + + // PID to reach setpoint using control_output + differential_drive_setpoint_s differential_drive_control_output = _differential_drive_setpoint; + + if (_differential_drive_setpoint.closed_loop_speed_control) { + differential_drive_control_output.speed += + pid_calculate(&_pid_speed, _differential_drive_setpoint.speed, _vehicle_forward_speed, 0, dt); + } + + if (_differential_drive_setpoint.closed_loop_yaw_rate_control) { + differential_drive_control_output.yaw_rate += + pid_calculate(&_pid_angular_velocity, _differential_drive_setpoint.yaw_rate, _vehicle_body_yaw_rate, 0, dt); + } + + differential_drive_control_output.timestamp = hrt_absolute_time(); + _differential_drive_control_output_pub.publish(differential_drive_control_output); +} diff --git a/src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.hpp b/src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.hpp new file mode 100644 index 000000000000..8d3b7e1c216c --- /dev/null +++ b/src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.hpp @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * Copyright (c) 2023-2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file DifferentialDriveControl.hpp + * + * Controller for heading rate and forward speed. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class DifferentialDriveControl : public ModuleParams +{ +public: + DifferentialDriveControl(ModuleParams *parent); + ~DifferentialDriveControl() = default; + + void control(float dt); + float getVehicleBodyYawRate() const { return _vehicle_body_yaw_rate; } + float getVehicleYaw() const { return _vehicle_yaw; } + +protected: + void updateParams() override; + +private: + uORB::Subscription _differential_drive_setpoint_sub{ORB_ID(differential_drive_setpoint)}; + uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + + uORB::Publication _differential_drive_control_output_pub{ORB_ID(differential_drive_control_output)}; + + differential_drive_setpoint_s _differential_drive_setpoint{}; + + matrix::Quatf _vehicle_attitude_quaternion{}; + float _vehicle_yaw{0.f}; + + // States + float _vehicle_body_yaw_rate{0.f}; + float _vehicle_forward_speed{0.f}; + + PID_t _pid_angular_velocity; ///< The PID controller for yaw rate. + PID_t _pid_speed; ///< The PID controller for velocity. + + DEFINE_PARAMETERS( + (ParamFloat) _param_rdd_p_gain_speed, + (ParamFloat) _param_rdd_i_gain_speed, + (ParamFloat) _param_rdd_p_gain_angular_velocity, + (ParamFloat) _param_rdd_i_gain_angular_velocity + ) +}; diff --git a/src/modules/differential_drive/DifferentialDriveGuidance/CMakeLists.txt b/src/modules/differential_drive/DifferentialDriveGuidance/CMakeLists.txt new file mode 100644 index 000000000000..fa833c50ae42 --- /dev/null +++ b/src/modules/differential_drive/DifferentialDriveGuidance/CMakeLists.txt @@ -0,0 +1,38 @@ +############################################################################ +# +# Copyright (c) 2023-2024 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(DifferentialDriveGuidance + DifferentialDriveGuidance.cpp +) + +target_include_directories(DifferentialDriveGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp b/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp new file mode 100644 index 000000000000..df453ff8b5c0 --- /dev/null +++ b/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp @@ -0,0 +1,138 @@ +/**************************************************************************** + * + * Copyright (c) 2023-2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "DifferentialDriveGuidance.hpp" + +#include + +using namespace matrix; + +DifferentialDriveGuidance::DifferentialDriveGuidance(ModuleParams *parent) : ModuleParams(parent) +{ + updateParams(); + + pid_init(&_heading_p_controller, PID_MODE_DERIVATIV_NONE, 0.001f); +} + +void DifferentialDriveGuidance::computeGuidance(float yaw, float angular_velocity, float dt) +{ + if (_position_setpoint_triplet_sub.updated()) { + _position_setpoint_triplet_sub.copy(&_position_setpoint_triplet); + } + + if (_vehicle_global_position_sub.updated()) { + _vehicle_global_position_sub.copy(&_vehicle_global_position); + } + + matrix::Vector2d global_position(_vehicle_global_position.lat, _vehicle_global_position.lon); + matrix::Vector2d current_waypoint(_position_setpoint_triplet.current.lat, _position_setpoint_triplet.current.lon); + matrix::Vector2d next_waypoint(_position_setpoint_triplet.next.lat, _position_setpoint_triplet.next.lon); + + const float distance_to_next_wp = get_distance_to_next_waypoint(global_position(0), global_position(1), + current_waypoint(0), + current_waypoint(1)); + + float desired_heading = get_bearing_to_next_waypoint(global_position(0), global_position(1), current_waypoint(0), + current_waypoint(1)); + float heading_error = matrix::wrap_pi(desired_heading - yaw); + + if (_current_waypoint != current_waypoint) { + _currentState = GuidanceState::TURNING; + } + + // Make rover stop when it arrives at the last waypoint instead of loitering and driving around weirdly. + if ((current_waypoint == next_waypoint) && distance_to_next_wp <= _param_nav_acc_rad.get()) { + _currentState = GuidanceState::GOAL_REACHED; + } + + float desired_speed = 0.f; + + switch (_currentState) { + case GuidanceState::TURNING: + desired_speed = 0.f; + + if (fabsf(heading_error) < 0.05f) { + _currentState = GuidanceState::DRIVING; + } + + break; + + case GuidanceState::DRIVING: { + const float max_velocity = math::trajectory::computeMaxSpeedFromDistance(_param_rdd_max_jerk.get(), + _param_rdd_max_accel.get(), distance_to_next_wp, 0.0f); + _forwards_velocity_smoothing.updateDurations(max_velocity); + _forwards_velocity_smoothing.updateTraj(dt); + desired_speed = math::interpolate(abs(heading_error), 0.1f, 0.8f, + _forwards_velocity_smoothing.getCurrentVelocity(), 0.0f); + break; + } + + case GuidanceState::GOAL_REACHED: + // temporary till I find a better way to stop the vehicle + desired_speed = 0.f; + heading_error = 0.f; + angular_velocity = 0.f; + _desired_angular_velocity = 0.f; + break; + } + + // Compute the desired angular velocity relative to the heading error, to steer the vehicle towards the next waypoint. + float angular_velocity_pid = pid_calculate(&_heading_p_controller, heading_error, angular_velocity, 0, + dt) + heading_error; + + differential_drive_setpoint_s output{}; + output.speed = math::constrain(desired_speed, -_max_speed, _max_speed); + output.yaw_rate = math::constrain(angular_velocity_pid, -_max_angular_velocity, _max_angular_velocity); + output.closed_loop_speed_control = output.closed_loop_yaw_rate_control = true; + output.timestamp = hrt_absolute_time(); + + _differential_drive_setpoint_pub.publish(output); + + _current_waypoint = current_waypoint; +} + +void DifferentialDriveGuidance::updateParams() +{ + ModuleParams::updateParams(); + + pid_set_parameters(&_heading_p_controller, + _param_rdd_p_gain_heading.get(), // Proportional gain + 0, // Integral gain + 0, // Derivative gain + 0, // Integral limit + 200); // Output limit + + _forwards_velocity_smoothing.setMaxJerk(_param_rdd_max_jerk.get()); + _forwards_velocity_smoothing.setMaxAccel(_param_rdd_max_accel.get()); + _forwards_velocity_smoothing.setMaxVel(_max_speed); +} diff --git a/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp b/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp new file mode 100644 index 000000000000..0a5a29bbca0b --- /dev/null +++ b/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp @@ -0,0 +1,140 @@ +/**************************************************************************** + * + * Copyright (c) 2023-2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +#include + + +/** + * @brief Enum class for the different states of guidance. + */ +enum class GuidanceState { + TURNING, ///< The vehicle is currently turning. + DRIVING, ///< The vehicle is currently driving straight. + GOAL_REACHED ///< The vehicle has reached its goal. +}; + +/** + * @brief Class for differential drive guidance. + */ +class DifferentialDriveGuidance : public ModuleParams +{ +public: + /** + * @brief Constructor for DifferentialDriveGuidance. + * @param parent The parent ModuleParams object. + */ + DifferentialDriveGuidance(ModuleParams *parent); + ~DifferentialDriveGuidance() = default; + + /** + * @brief Compute guidance for the vehicle. + * @param global_pos The global position of the vehicle in degrees. + * @param current_waypoint The current waypoint the vehicle is heading towards in degrees. + * @param next_waypoint The next waypoint the vehicle will head towards after reaching the current waypoint in degrees. + * @param vehicle_yaw The yaw orientation of the vehicle in radians. + * @param body_velocity The velocity of the vehicle in m/s. + * @param angular_velocity The angular velocity of the vehicle in rad/s. + * @param dt The time step in seconds. + */ + void computeGuidance(float yaw, float angular_velocity, float dt); + + /** + * @brief Set the maximum speed for the vehicle. + * @param max_speed The maximum speed in m/s. + * @return The set maximum speed in m/s. + */ + float setMaxSpeed(float max_speed) { return _max_speed = max_speed; } + + + /** + * @brief Set the maximum angular velocity for the vehicle. + * @param max_angular_velocity The maximum angular velocity in rad/s. + * @return The set maximum angular velocity in rad/s. + */ + float setMaxAngularVelocity(float max_angular_velocity) { return _max_angular_velocity = max_angular_velocity; } + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; + uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; + + uORB::Publication _differential_drive_setpoint_pub{ORB_ID(differential_drive_setpoint)}; + + position_setpoint_triplet_s _position_setpoint_triplet{}; + vehicle_global_position_s _vehicle_global_position{}; + + GuidanceState _currentState; ///< The current state of guidance. + + float _desired_angular_velocity; ///< The desired angular velocity. + + float _max_speed; ///< The maximum speed. + float _max_angular_velocity; ///< The maximum angular velocity. + + matrix::Vector2d _current_waypoint; ///< The current waypoint. + + VelocitySmoothing _forwards_velocity_smoothing; ///< The velocity smoothing for forward motion. + PositionSmoothing _position_smoothing; ///< The position smoothing. + + PID_t _heading_p_controller; ///< The PID controller for yaw rate. + + DEFINE_PARAMETERS( + (ParamFloat) _param_rdd_p_gain_heading, + (ParamFloat) _param_nav_acc_rad, + (ParamFloat) _param_rdd_max_jerk, + (ParamFloat) _param_rdd_max_accel + ) +}; diff --git a/src/modules/differential_drive_control/DifferentialDriveKinematics/CMakeLists.txt b/src/modules/differential_drive/DifferentialDriveKinematics/CMakeLists.txt similarity index 91% rename from src/modules/differential_drive_control/DifferentialDriveKinematics/CMakeLists.txt rename to src/modules/differential_drive/DifferentialDriveKinematics/CMakeLists.txt index c352d7c59758..c556db2b8f3f 100644 --- a/src/modules/differential_drive_control/DifferentialDriveKinematics/CMakeLists.txt +++ b/src/modules/differential_drive/DifferentialDriveKinematics/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2023 PX4 Development Team. All rights reserved. +# Copyright (c) 2023-2024 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -37,4 +37,4 @@ px4_add_library(DifferentialDriveKinematics target_include_directories(DifferentialDriveKinematics PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -px4_add_unit_gtest(SRC DifferentialDriveKinematicsTest.cpp LINKLIBS DifferentialDriveKinematics) +px4_add_functional_gtest(SRC DifferentialDriveKinematicsTest.cpp LINKLIBS DifferentialDriveKinematics) diff --git a/src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp b/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp similarity index 68% rename from src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp rename to src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp index ffa96cbe9004..42fcbea56b66 100644 --- a/src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp +++ b/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2023 PX4 Development Team. All rights reserved. + * Copyright (C) 2023-2024 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,8 +36,38 @@ #include using namespace matrix; +using namespace time_literals; -matrix::Vector2f DifferentialDriveKinematics::computeInverseKinematics(float linear_velocity_x, float yaw_rate) +DifferentialDriveKinematics::DifferentialDriveKinematics(ModuleParams *parent) : ModuleParams(parent) +{} + +void DifferentialDriveKinematics::allocate() +{ + hrt_abstime now = hrt_absolute_time(); + + if (_differential_drive_control_output_sub.updated()) { + _differential_drive_control_output_sub.copy(&_differential_drive_control_output); + } + + const bool setpoint_timeout = (_differential_drive_control_output.timestamp + 100_ms) < now; + + Vector2f wheel_speeds = + computeInverseKinematics(_differential_drive_control_output.speed, _differential_drive_control_output.yaw_rate); + + if (!_armed || setpoint_timeout) { + wheel_speeds = {}; // stop + } + + wheel_speeds = matrix::constrain(wheel_speeds, -1.f, 1.f); + + actuator_motors_s actuator_motors{}; + actuator_motors.reversible_flags = _param_r_rev.get(); // should be 3 see rc.rover_differential_defaults + wheel_speeds.copyTo(actuator_motors.control); + actuator_motors.timestamp = now; + _actuator_motors_pub.publish(actuator_motors); +} + +matrix::Vector2f DifferentialDriveKinematics::computeInverseKinematics(float linear_velocity_x, float yaw_rate) const { if (_max_speed < FLT_EPSILON) { return Vector2f(); @@ -54,7 +84,7 @@ matrix::Vector2f DifferentialDriveKinematics::computeInverseKinematics(float lin if (combined_velocity > _max_speed) { float excess_velocity = fabsf(combined_velocity - _max_speed); - float adjusted_linear_velocity = fabsf(linear_velocity_x) - excess_velocity; + const float adjusted_linear_velocity = fabsf(linear_velocity_x) - excess_velocity; gain = adjusted_linear_velocity / fabsf(linear_velocity_x); } @@ -65,4 +95,3 @@ matrix::Vector2f DifferentialDriveKinematics::computeInverseKinematics(float lin return Vector2f(linear_velocity_x - rotational_velocity, linear_velocity_x + rotational_velocity) / _max_speed; } - diff --git a/src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp b/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp similarity index 78% rename from src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp rename to src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp index 193a7d05fb8e..524a5520a78f 100644 --- a/src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp +++ b/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * Copyright (c) 2023-2024 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,6 +34,11 @@ #pragma once #include +#include +#include +#include +#include +#include /** * @brief Differential Drive Kinematics class for computing the kinematics of a differential drive robot. @@ -41,10 +46,10 @@ * This class provides functions to set the wheel base and radius, and to compute the inverse kinematics * given linear velocity and yaw rate. */ -class DifferentialDriveKinematics +class DifferentialDriveKinematics : public ModuleParams { public: - DifferentialDriveKinematics() = default; + DifferentialDriveKinematics(ModuleParams *parent); ~DifferentialDriveKinematics() = default; /** @@ -68,6 +73,10 @@ class DifferentialDriveKinematics */ void setMaxAngularVelocity(const float max_angular_velocity) { _max_angular_velocity = max_angular_velocity; }; + void setArmed(const bool armed) { _armed = armed; }; + + void allocate(); + /** * @brief Computes the inverse kinematics for differential drive. * @@ -75,10 +84,20 @@ class DifferentialDriveKinematics * @param yaw_rate Yaw rate of the robot. * @return matrix::Vector2f Motor velocities for the right and left motors. */ - matrix::Vector2f computeInverseKinematics(float linear_velocity_x, float yaw_rate); + matrix::Vector2f computeInverseKinematics(float linear_velocity_x, float yaw_rate) const; private: + uORB::Subscription _differential_drive_control_output_sub{ORB_ID(differential_drive_control_output)}; + uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; + + differential_drive_setpoint_s _differential_drive_control_output{}; + bool _armed = false; + float _wheel_base{0.f}; float _max_speed{0.f}; float _max_angular_velocity{0.f}; + + DEFINE_PARAMETERS( + (ParamInt) _param_r_rev + ) }; diff --git a/src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp b/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp similarity index 80% rename from src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp rename to src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp index 6eee43c41ebc..b3c4935c1d9d 100644 --- a/src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp +++ b/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2023 PX4 Development Team. All rights reserved. + * Copyright (C) 2023-2024 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,9 +37,14 @@ using namespace matrix; -TEST(DifferentialDriveKinematicsTest, AllZeroInputCase) +class DifferentialDriveKinematicsTest : public ::testing::Test +{ +public: + DifferentialDriveKinematics kinematics{nullptr}; +}; + +TEST_F(DifferentialDriveKinematicsTest, AllZeroInputCase) { - DifferentialDriveKinematics kinematics; kinematics.setWheelBase(1.f); kinematics.setMaxSpeed(10.f); kinematics.setMaxAngularVelocity(10.f); @@ -49,9 +54,8 @@ TEST(DifferentialDriveKinematicsTest, AllZeroInputCase) } -TEST(DifferentialDriveKinematicsTest, InvalidParameterCase) +TEST_F(DifferentialDriveKinematicsTest, InvalidParameterCase) { - DifferentialDriveKinematics kinematics; kinematics.setWheelBase(0.f); kinematics.setMaxSpeed(10.f); kinematics.setMaxAngularVelocity(10.f); @@ -61,9 +65,8 @@ TEST(DifferentialDriveKinematicsTest, InvalidParameterCase) } -TEST(DifferentialDriveKinematicsTest, UnitCase) +TEST_F(DifferentialDriveKinematicsTest, UnitCase) { - DifferentialDriveKinematics kinematics; kinematics.setWheelBase(1.f); kinematics.setMaxSpeed(10.f); kinematics.setMaxAngularVelocity(10.f); @@ -73,9 +76,8 @@ TEST(DifferentialDriveKinematicsTest, UnitCase) } -TEST(DifferentialDriveKinematicsTest, UnitSaturationCase) +TEST_F(DifferentialDriveKinematicsTest, UnitSaturationCase) { - DifferentialDriveKinematics kinematics; kinematics.setWheelBase(1.f); kinematics.setMaxSpeed(1.f); kinematics.setMaxAngularVelocity(1.f); @@ -85,9 +87,8 @@ TEST(DifferentialDriveKinematicsTest, UnitSaturationCase) } -TEST(DifferentialDriveKinematicsTest, OppositeUnitSaturationCase) +TEST_F(DifferentialDriveKinematicsTest, OppositeUnitSaturationCase) { - DifferentialDriveKinematics kinematics; kinematics.setWheelBase(1.f); kinematics.setMaxSpeed(1.f); kinematics.setMaxAngularVelocity(1.f); @@ -96,9 +97,8 @@ TEST(DifferentialDriveKinematicsTest, OppositeUnitSaturationCase) EXPECT_EQ(kinematics.computeInverseKinematics(-1.f, 1.f), Vector2f(-1, 0)); } -TEST(DifferentialDriveKinematicsTest, RandomCase) +TEST_F(DifferentialDriveKinematicsTest, RandomCase) { - DifferentialDriveKinematics kinematics; kinematics.setWheelBase(2.f); kinematics.setMaxSpeed(1.f); kinematics.setMaxAngularVelocity(1.f); @@ -107,9 +107,8 @@ TEST(DifferentialDriveKinematicsTest, RandomCase) EXPECT_EQ(kinematics.computeInverseKinematics(0.5f, 0.7f), Vector2f(-0.4f, 1.0f)); } -TEST(DifferentialDriveKinematicsTest, RotateInPlaceCase) +TEST_F(DifferentialDriveKinematicsTest, RotateInPlaceCase) { - DifferentialDriveKinematics kinematics; kinematics.setWheelBase(1.f); kinematics.setMaxSpeed(1.f); kinematics.setMaxAngularVelocity(1.f); @@ -118,9 +117,8 @@ TEST(DifferentialDriveKinematicsTest, RotateInPlaceCase) EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 1.f), Vector2f(-0.5f, 0.5f)); } -TEST(DifferentialDriveKinematicsTest, StraightMovementCase) +TEST_F(DifferentialDriveKinematicsTest, StraightMovementCase) { - DifferentialDriveKinematics kinematics; kinematics.setWheelBase(1.f); kinematics.setMaxSpeed(1.f); kinematics.setMaxAngularVelocity(1.f); @@ -129,9 +127,8 @@ TEST(DifferentialDriveKinematicsTest, StraightMovementCase) EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 0.f), Vector2f(1.f, 1.f)); } -TEST(DifferentialDriveKinematicsTest, MinInputValuesCase) +TEST_F(DifferentialDriveKinematicsTest, MinInputValuesCase) { - DifferentialDriveKinematics kinematics; kinematics.setWheelBase(FLT_MIN); kinematics.setMaxSpeed(FLT_MIN); kinematics.setMaxAngularVelocity(FLT_MIN); @@ -140,9 +137,8 @@ TEST(DifferentialDriveKinematicsTest, MinInputValuesCase) EXPECT_EQ(kinematics.computeInverseKinematics(FLT_MIN, FLT_MIN), Vector2f(0.f, 0.f)); } -TEST(DifferentialDriveKinematicsTest, MaxSpeedLimitCase) +TEST_F(DifferentialDriveKinematicsTest, MaxSpeedLimitCase) { - DifferentialDriveKinematics kinematics; kinematics.setWheelBase(1.f); kinematics.setMaxSpeed(1.f); kinematics.setMaxAngularVelocity(1.f); @@ -151,9 +147,8 @@ TEST(DifferentialDriveKinematicsTest, MaxSpeedLimitCase) EXPECT_EQ(kinematics.computeInverseKinematics(10.f, 10.f), Vector2f(0.f, 1.f)); } -TEST(DifferentialDriveKinematicsTest, MaxSpeedForwardsCase) +TEST_F(DifferentialDriveKinematicsTest, MaxSpeedForwardsCase) { - DifferentialDriveKinematics kinematics; kinematics.setWheelBase(1.f); kinematics.setMaxSpeed(1.f); kinematics.setMaxAngularVelocity(1.f); @@ -162,9 +157,8 @@ TEST(DifferentialDriveKinematicsTest, MaxSpeedForwardsCase) EXPECT_EQ(kinematics.computeInverseKinematics(10.f, 0.f), Vector2f(1.f, 1.f)); } -TEST(DifferentialDriveKinematicsTest, MaxAngularCase) +TEST_F(DifferentialDriveKinematicsTest, MaxAngularCase) { - DifferentialDriveKinematics kinematics; kinematics.setWheelBase(2.f); kinematics.setMaxSpeed(1.f); kinematics.setMaxAngularVelocity(1.f); diff --git a/src/modules/differential_drive_control/Kconfig b/src/modules/differential_drive/Kconfig similarity index 59% rename from src/modules/differential_drive_control/Kconfig rename to src/modules/differential_drive/Kconfig index 9d4a0b54681d..a95897e91f99 100644 --- a/src/modules/differential_drive_control/Kconfig +++ b/src/modules/differential_drive/Kconfig @@ -1,5 +1,5 @@ -menuconfig MODULES_DIFFERENTIAL_DRIVE_CONTROL - bool "differential_drive_control" +menuconfig MODULES_DIFFERENTIAL_DRIVE + bool "differential_drive" default n depends on MODULES_CONTROL_ALLOCATOR ---help--- diff --git a/src/modules/differential_drive/module.yaml b/src/modules/differential_drive/module.yaml new file mode 100644 index 000000000000..50ab9bb837ca --- /dev/null +++ b/src/modules/differential_drive/module.yaml @@ -0,0 +1,122 @@ +module_name: Differential Drive + +parameters: + - group: Rover Differential Drive + definitions: + RDD_WHEEL_BASE: + description: + short: Wheel base + long: Distance from the center of the right wheel to the center of the left wheel + type: float + unit: m + min: 0.001 + max: 100 + increment: 0.001 + decimal: 3 + default: 0.5 + RDD_WHEEL_RADIUS: + description: + short: Wheel radius + long: Size of the wheel, half the diameter of the wheel + type: float + unit: m + min: 0.001 + max: 100 + increment: 0.001 + decimal: 3 + default: 0.1 + RDD_SPEED_SCALE: + description: + short: Manual speed scale + type: float + min: 0 + max: 1 + increment: 0.01 + decimal: 2 + default: 1 + RDD_ANG_SCALE: + description: + short: Manual angular velocity scale + type: float + min: 0 + max: 1 + increment: 0.01 + decimal: 2 + default: 1 + RDD_WHEEL_SPEED: + description: + short: Maximum wheel speed + type: float + unit: rad/s + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 0.3 + RDD_P_HEADING: + description: + short: Proportional gain for heading controller + type: float + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 1 + RDD_P_SPEED: + description: + short: Proportional gain for speed controller + type: float + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 1 + RDD_I_SPEED: + description: + short: Integral gain for ground speed controller + type: float + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 0 + RDD_P_ANG_VEL: + description: + short: Proportional gain for angular velocity controller + type: float + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 1 + RDD_I_ANG_VEL: + description: + short: Integral gain for angular velocity controller + type: float + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 0 + RDD_MAX_JERK: + description: + short: Maximum jerk + long: Limit for forwards acc/deceleration change. + type: float + unit: m/s^3 + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 0.5 + RDD_MAX_ACCEL: + description: + short: Maximum acceleration + long: Maximum acceleration is used to limit the acceleration of the rover + type: float + unit: m/s^2 + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 0.5 diff --git a/src/modules/differential_drive_control/module.yaml b/src/modules/differential_drive_control/module.yaml deleted file mode 100644 index 77bd61dda4dc..000000000000 --- a/src/modules/differential_drive_control/module.yaml +++ /dev/null @@ -1,55 +0,0 @@ -module_name: Differential Drive Control - -parameters: - - group: Rover Differential Drive - definitions: - RDD_WHEEL_BASE: - description: - short: Wheel base - long: Distance from the center of the right wheel to the center of the left wheel - type: float - unit: m - min: 0.001 - max: 100 - increment: 0.001 - decimal: 3 - default: 0.5 - RDD_WHEEL_RADIUS: - description: - short: Wheel radius - long: Size of the wheel, half the diameter of the wheel - type: float - unit: m - min: 0.001 - max: 100 - increment: 0.001 - decimal: 3 - default: 0.1 - RDD_SPEED_SCALE: - description: - short: Manual speed scale - type: float - min: 0 - max: 1.0 - increment: 0.01 - decimal: 2 - default: 1.0 - RDD_ANG_SCALE: - description: - short: Manual angular velocity scale - type: float - min: 0 - max: 1.0 - increment: 0.01 - decimal: 2 - default: 1.0 - RDD_WHL_SPEED: - description: - short: Maximum wheel speed - type: float - unit: rad/s - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 10 diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index abd1bab9d672..3f1f9a4c991d 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -127,7 +127,8 @@ list(APPEND EKF_SRCS EKF/height_control.cpp EKF/imu_down_sampler.cpp EKF/output_predictor.cpp - EKF/vel_pos_fusion.cpp + EKF/velocity_fusion.cpp + EKF/position_fusion.cpp EKF/yaw_fusion.cpp EKF/zero_innovation_heading_update.cpp diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index f3c018c5ed7f..adf922907a1f 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -44,7 +44,8 @@ list(APPEND EKF_SRCS height_control.cpp imu_down_sampler.cpp output_predictor.cpp - vel_pos_fusion.cpp + velocity_fusion.cpp + position_fusion.cpp yaw_fusion.cpp zero_innovation_heading_update.cpp diff --git a/src/modules/ekf2/EKF/EKFGSF_yaw.h b/src/modules/ekf2/EKF/EKFGSF_yaw.h index 3719aec7cebd..7add1dedef7a 100644 --- a/src/modules/ekf2/EKF/EKFGSF_yaw.h +++ b/src/modules/ekf2/EKF/EKFGSF_yaw.h @@ -67,11 +67,11 @@ class EKFGSF_yaw void setTrueAirspeed(float true_airspeed) { _true_airspeed = true_airspeed; } - void setGyroBias(const Vector3f &imu_gyro_bias) + void setGyroBias(const Vector3f &imu_gyro_bias, const bool force = false) { // Initialise to gyro bias estimate from main filter because there could be a large // uncorrected rate gyro bias error about the gravity vector - if (!_ahrs_ekf_gsf_tilt_aligned || !_ekf_gsf_vel_fuse_started) { + if (!_ahrs_ekf_gsf_tilt_aligned || !_ekf_gsf_vel_fuse_started || force) { // init gyro bias for each model for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) { _ahrs_ekf_gsf[model_index].gyro_bias = imu_gyro_bias; diff --git a/src/modules/ekf2/EKF/RingBuffer.h b/src/modules/ekf2/EKF/RingBuffer.h index aeb4471ad586..2e4ff02bb3ee 100644 --- a/src/modules/ekf2/EKF/RingBuffer.h +++ b/src/modules/ekf2/EKF/RingBuffer.h @@ -153,6 +153,7 @@ class RingBuffer return false; } + int get_used_size() const { return sizeof(*this) + sizeof(data_type) * entries(); } int get_total_size() const { return sizeof(*this) + sizeof(data_type) * _size; } int entries() const diff --git a/src/modules/ekf2/EKF/aid_sources/ZeroGyroUpdate.cpp b/src/modules/ekf2/EKF/aid_sources/ZeroGyroUpdate.cpp index ea9d3f11a8a3..b9eea136042f 100644 --- a/src/modules/ekf2/EKF/aid_sources/ZeroGyroUpdate.cpp +++ b/src/modules/ekf2/EKF/aid_sources/ZeroGyroUpdate.cpp @@ -60,22 +60,13 @@ bool ZeroGyroUpdate::update(Ekf &ekf, const estimator::imuSample &imu_delayed) if (zero_gyro_update_data_ready) { Vector3f gyro_bias = _zgup_delta_ang / _zgup_delta_ang_dt; - Vector3f innovation = ekf.state().gyro_bias - gyro_bias; const float obs_var = sq(math::constrain(ekf.getGyroNoise(), 0.f, 1.f)); - const Vector3f innov_var = ekf.getGyroBiasVariance() + obs_var; - - for (int i = 0; i < 3; i++) { - Ekf::VectorState K; // Kalman gain vector for any single observation - sequential fusion is used. - const unsigned state_index = State::gyro_bias.idx + i; - - // calculate kalman gain K = PHS, where S = 1/innovation variance - for (int row = 0; row < State::size; row++) { - K(row) = ekf.stateCovariance(row, state_index) / innov_var(i); - } - - ekf.measurementUpdate(K, innov_var(i), innovation(i)); + for (unsigned i = 0; i < 3; i++) { + const float innovation = ekf.state().gyro_bias(i) - gyro_bias(i); + const float innov_var = ekf.getGyroBiasVariance()(i) + obs_var; + ekf.fuseDirectStateMeasurement(innovation, innov_var, obs_var, State::gyro_bias.idx + i); } // Reset the integrators diff --git a/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.cpp b/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.cpp index d091e5aadb64..48d07d583dac 100644 --- a/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.cpp +++ b/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.cpp @@ -58,16 +58,15 @@ bool ZeroVelocityUpdate::update(Ekf &ekf, const estimator::imuSample &imu_delaye if (continuing_conditions_passing) { Vector3f vel_obs{0.f, 0.f, 0.f}; - Vector3f innovation = ekf.state().vel - vel_obs; // Set a low variance initially for faster leveling and higher // later to let the states follow the measurements - const float obs_var = ekf.control_status_flags().tilt_align ? sq(0.2f) : sq(0.001f); Vector3f innov_var = ekf.getVelocityVariance() + obs_var; for (unsigned i = 0; i < 3; i++) { - ekf.fuseVelPosHeight(innovation(i), innov_var(i), State::vel.idx + i); + const float innovation = ekf.state().vel(i) - vel_obs(i); + ekf.fuseDirectStateMeasurement(innovation, innov_var(i), obs_var, State::vel.idx + i); } _time_last_zero_velocity_fuse = imu_delayed.time_us; diff --git a/src/modules/ekf2/EKF/airspeed_fusion.cpp b/src/modules/ekf2/EKF/airspeed_fusion.cpp index fde836232b34..7f8539b1cb0e 100644 --- a/src/modules/ekf2/EKF/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/airspeed_fusion.cpp @@ -93,7 +93,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed) const bool is_airspeed_significant = airspeed_sample.true_airspeed > _params.arsp_thr; const bool is_airspeed_consistent = (_aid_src_airspeed.test_ratio > 0.f && _aid_src_airspeed.test_ratio < 1.f); const bool starting_conditions_passing = continuing_conditions_passing && is_airspeed_significant - && (is_airspeed_consistent || !_control_status.flags.wind); // if wind isn't already estimated, the states are reset when starting airspeed fusion + && (is_airspeed_consistent || !_control_status.flags.wind || _control_status.flags.inertial_dead_reckoning); if (_control_status.flags.fuse_aspd) { if (continuing_conditions_passing) { @@ -118,17 +118,16 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed) } else if (starting_conditions_passing) { ECL_INFO("starting airspeed fusion"); - // If starting wind state estimation, reset the wind states and covariances before fusing any data - // Also catch the case where sideslip fusion enabled wind estimation recently and didn't converge yet. - const Vector2f wind_var_xy = getWindVelocityVariance(); + if (_control_status.flags.inertial_dead_reckoning && !is_airspeed_consistent) { + resetVelUsingAirspeed(airspeed_sample); - if (!_control_status.flags.wind || (wind_var_xy(0) + wind_var_xy(1) > sq(_params.initial_wind_uncertainty))) { - // activate the wind states - _control_status.flags.wind = true; - // reset the wind speed states and corresponding covariances + } else if (!_control_status.flags.wind || getWindVelocityVariance().longerThan(_params.initial_wind_uncertainty)) { + // If starting wind state estimation, reset the wind states and covariances before fusing any data + // Also catch the case where sideslip fusion enabled wind estimation recently and didn't converge yet. resetWindUsingAirspeed(airspeed_sample); } + _control_status.flags.wind = true; _control_status.flags.fuse_aspd = true; } @@ -208,7 +207,7 @@ void Ekf::fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_sour K.slice(State::wind_vel.idx, 0) = K_wind; } - const bool is_fused = measurementUpdate(K, aid_src.innovation_variance, aid_src.innovation); + const bool is_fused = measurementUpdate(K, H, aid_src.observation_variance, aid_src.innovation); aid_src.fused = is_fused; _fault_status.flags.bad_airspeed = !is_fused; @@ -247,3 +246,18 @@ void Ekf::resetWindUsingAirspeed(const airspeedSample &airspeed_sample) _aid_src_airspeed.time_last_fuse = _time_delayed_us; } + +void Ekf::resetVelUsingAirspeed(const airspeedSample &airspeed_sample) +{ + const float euler_yaw = getEulerYaw(_R_to_earth); + + // Estimate velocity using zero sideslip assumption and airspeed measurement + Vector2f horizontal_velocity; + horizontal_velocity(0) = _state.wind_vel(0) + airspeed_sample.true_airspeed * cosf(euler_yaw); + horizontal_velocity(1) = _state.wind_vel(1) + airspeed_sample.true_airspeed * sinf(euler_yaw); + + float vel_var = NAN; // Do not reset the velocity variance as wind variance estimate is most likely not correct + resetHorizontalVelocityTo(horizontal_velocity, vel_var); + + _aid_src_airspeed.time_last_fuse = _time_delayed_us; +} diff --git a/src/modules/ekf2/EKF/aux_global_position.cpp b/src/modules/ekf2/EKF/aux_global_position.cpp index 539643513c27..f64a990bae84 100644 --- a/src/modules/ekf2/EKF/aux_global_position.cpp +++ b/src/modules/ekf2/EKF/aux_global_position.cpp @@ -56,6 +56,7 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed sample.altitude_amsl = aux_global_position.alt; sample.eph = aux_global_position.eph; sample.epv = aux_global_position.epv; + sample.lat_lon_reset_counter = aux_global_position.lat_lon_reset_counter; _aux_global_position_buffer.push(sample); @@ -103,12 +104,14 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed if (ekf.global_origin_valid()) { ekf.enableControlStatusAuxGpos(); + _reset_counters.lat_lon = sample.lat_lon_reset_counter; _state = State::active; } else { // Try to initialize using measurement if (ekf.setEkfGlobalOrigin(sample.latitude, sample.longitude, sample.altitude_amsl, sample.eph, sample.epv)) { ekf.enableControlStatusAuxGpos(); + _reset_counters.lat_lon = sample.lat_lon_reset_counter; _state = State::active; } } @@ -119,6 +122,13 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed if (continuing_conditions) { ekf.fuseHorizontalPosition(aid_src); + if (isTimedOut(aid_src.time_last_fuse, imu_delayed.time_us, ekf._params.no_aid_timeout_max) + || (_reset_counters.lat_lon != sample.lat_lon_reset_counter)) { + ekf.resetHorizontalPositionTo(Vector2f(aid_src.observation), Vector2f(aid_src.observation_variance)); + aid_src.time_last_fuse = imu_delayed.time_us; + _reset_counters.lat_lon = sample.lat_lon_reset_counter; + } + } else { ekf.disableControlStatusAuxGpos(); _state = State::stopped; diff --git a/src/modules/ekf2/EKF/aux_global_position.hpp b/src/modules/ekf2/EKF/aux_global_position.hpp index 4f1355483f16..4246477350aa 100644 --- a/src/modules/ekf2/EKF/aux_global_position.hpp +++ b/src/modules/ekf2/EKF/aux_global_position.hpp @@ -60,7 +60,11 @@ class Ekf; class AuxGlobalPosition : public ModuleParams { public: - AuxGlobalPosition() : ModuleParams(nullptr) {} + AuxGlobalPosition() : ModuleParams(nullptr) + { + _estimator_aid_src_aux_global_position_pub.advertise(); + } + ~AuxGlobalPosition() = default; void update(Ekf &ekf, const estimator::imuSample &imu_delayed); @@ -83,6 +87,7 @@ class AuxGlobalPosition : public ModuleParams float altitude_amsl{}; float eph{}; float epv{}; + uint8_t lat_lon_reset_counter{}; }; RingBuffer _aux_global_position_buffer{20}; // TODO: size with _obs_buffer_length and actual publication rate @@ -102,6 +107,11 @@ class AuxGlobalPosition : public ModuleParams State _state{State::stopped}; #if defined(MODULE_NAME) + struct reset_counters_s { + uint8_t lat_lon{}; + }; + reset_counters_s _reset_counters{}; + uORB::PublicationMulti _estimator_aid_src_aux_global_position_pub{ORB_ID(estimator_aid_src_aux_global_position)}; uORB::Subscription _aux_global_position_sub{ORB_ID(aux_global_position)}; diff --git a/src/modules/ekf2/EKF/auxvel_fusion.cpp b/src/modules/ekf2/EKF/auxvel_fusion.cpp index dbe88ccb87df..4ef94e31ee9e 100644 --- a/src/modules/ekf2/EKF/auxvel_fusion.cpp +++ b/src/modules/ekf2/EKF/auxvel_fusion.cpp @@ -42,10 +42,10 @@ void Ekf::controlAuxVelFusion() resetEstimatorAidStatus(_aid_src_aux_vel); - updateVelocityAidSrcStatus(auxvel_sample_delayed.time_us, auxvel_sample_delayed.vel, auxvel_sample_delayed.velVar, fmaxf(_params.auxvel_gate, 1.f), _aid_src_aux_vel); + updateHorizontalVelocityAidSrcStatus(auxvel_sample_delayed.time_us, auxvel_sample_delayed.vel, auxvel_sample_delayed.velVar, fmaxf(_params.auxvel_gate, 1.f), _aid_src_aux_vel); if (isHorizontalAidingActive()) { - fuseVelocity(_aid_src_aux_vel); + fuseHorizontalVelocity(_aid_src_aux_vel); } } } diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 1b9a741f369a..880df1d4fec7 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -364,7 +364,6 @@ struct parameters { int32_t mag_declination_source{7}; ///< bitmask used to control the handling of declination data int32_t mag_fusion_type{0}; ///< integer used to specify the type of magnetometer fusion used float mag_acc_gate{0.5f}; ///< when in auto select mode, heading fusion will be used when manoeuvre accel is lower than this (m/sec**2) - float mag_yaw_rate_gate{0.20f}; ///< yaw rate threshold used by mode select logic (rad/sec) // compute synthetic magnetomter Z value if possible int32_t synthesize_mag_z{0}; @@ -509,15 +508,9 @@ union fault_status_u { bool bad_sideslip : 1; ///< 6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error bool bad_optflow_X : 1; ///< 7 - true if fusion of the optical flow X axis has encountered a numerical error bool bad_optflow_Y : 1; ///< 8 - true if fusion of the optical flow Y axis has encountered a numerical error - bool bad_vel_N : 1; ///< 9 - true if fusion of the North velocity has encountered a numerical error - bool bad_vel_E : 1; ///< 10 - true if fusion of the East velocity has encountered a numerical error - bool bad_vel_D : 1; ///< 11 - true if fusion of the Down velocity has encountered a numerical error - bool bad_pos_N : 1; ///< 12 - true if fusion of the North position has encountered a numerical error - bool bad_pos_E : 1; ///< 13 - true if fusion of the East position has encountered a numerical error - bool bad_pos_D : 1; ///< 14 - true if fusion of the Down position has encountered a numerical error - bool bad_acc_bias : 1; ///< 15 - true if bad delta velocity bias estimates have been detected - bool bad_acc_vertical : 1; ///< 16 - true if bad vertical accelerometer data has been detected - bool bad_acc_clipping : 1; ///< 17 - true if delta velocity data contains clipping (asymmetric railing) + bool bad_acc_bias : 1; ///< 9 - true if bad delta velocity bias estimates have been detected + bool bad_acc_vertical : 1; ///< 10 - true if bad vertical accelerometer data has been detected + bool bad_acc_clipping : 1; ///< 11 - true if delta velocity data contains clipping (asymmetric railing) } flags; uint32_t value; }; @@ -655,6 +648,7 @@ union information_event_status_u { bool reset_hgt_to_gps : 1; ///< 14 - true when the vertical position state is reset to the gps measurement bool reset_hgt_to_rng : 1; ///< 15 - true when the vertical position state is reset to the rng measurement bool reset_hgt_to_ev : 1; ///< 16 - true when the vertical position state is reset to the ev measurement + bool reset_pos_to_ext_obs : 1; ///< 17 - true when horizontal position was reset to an external observation while deadreckoning } flags; uint32_t value; }; diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 2ab26658c3c3..34d26273e78b 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -68,11 +68,9 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) // monitor the tilt alignment if (!_control_status.flags.tilt_align) { // whilst we are aligning the tilt, monitor the variances - const Vector3f angle_err_var_vec = getQuaternionVariance(); - - // Once the tilt variances have reduced to equivalent of 3deg uncertainty + // Once the tilt variances have reduced to equivalent of 3 deg uncertainty // and declare the tilt alignment complete - if ((angle_err_var_vec(0) + angle_err_var_vec(1)) < sq(math::radians(3.0f))) { + if (getTiltVariance() < sq(math::radians(3.f))) { _control_status.flags.tilt_align = true; // send alignment status message to the console diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index aec4082cc188..c6ba03f25622 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -103,14 +103,14 @@ void Ekf::initialiseCovariance() 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; + // predict the covariance + const float dt = 0.5f * (imu_delayed.delta_vel_dt + imu_delayed.delta_ang_dt); - // delta angle noise variance + // gyro noise variance float gyro_noise = math::constrain(_params.gyro_noise, 0.f, 1.f); const float gyro_var = sq(gyro_noise); - // delta velocity noise variance + // accel noise variance float accel_noise = math::constrain(_params.accel_noise, 0.f, 1.f); Vector3f accel_var; @@ -124,207 +124,115 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) } } - // predict the covariance // calculate variances and upper diagonal covariances for quaternion, velocity, position and gyro bias states P = sym::PredictCovariance(_state.vector(), P, - imu_delayed.delta_vel / math::max(imu_delayed.delta_vel_dt, FLT_EPSILON), accel_var, - imu_delayed.delta_ang / math::max(imu_delayed.delta_ang_dt, FLT_EPSILON), gyro_var, - 0.5f * (imu_delayed.delta_vel_dt + imu_delayed.delta_ang_dt)); + imu_delayed.delta_vel / math::max(imu_delayed.delta_vel_dt, FLT_EPSILON), accel_var, + imu_delayed.delta_ang / math::max(imu_delayed.delta_ang_dt, FLT_EPSILON), gyro_var, + dt); // Construct the process noise variance diagonal for those states with a stationary process model // These are kinematic states and their error growth is controlled separately by the IMU noise variances - // gyro bias: add process noise, 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; + // gyro bias: add process noise unless state is 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); - if (!_gyro_bias_inhibit[index]) { - P(i, i) += gyro_bias_process_noise; + for (unsigned index = 0; index < State::gyro_bias.dof; index++) { + const unsigned i = State::gyro_bias.idx + index; - } else { - P.uncorrelateCovarianceSetVariance<1>(i, _prev_gyro_bias_var(index)); + if (!_gyro_bias_inhibit[index]) { + P(i, i) += gyro_bias_process_noise; + } } } - // 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; + // accel bias: add process noise unless state is 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); - if (!_accel_bias_inhibit[index]) { - P(i, i) += accel_bias_process_noise; + for (unsigned index = 0; index < State::accel_bias.dof; index++) { + const unsigned i = State::accel_bias.idx + index; - } else { - P.uncorrelateCovarianceSetVariance<1>(i, _prev_accel_bias_var(index)); + if (!_accel_bias_inhibit[index]) { + P(i, i) += accel_bias_process_noise; + } } } + #if defined(CONFIG_EKF2_MAGNETOMETER) if (_control_status.flags.mag) { - // 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 - if (P.trace(State::mag_I.idx) < 0.1f) { - - float mag_I_sig = dt * math::constrain(_params.mage_p_noise, 0.f, 1.f); - float mag_I_process_noise = sq(mag_I_sig); + // mag_I: add process noise + float mag_I_sig = dt * math::constrain(_params.mage_p_noise, 0.f, 1.f); + 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; - P(i, i) += mag_I_process_noise; - } + for (unsigned index = 0; index < State::mag_I.dof; index++) { + const unsigned i = State::mag_I.idx + index; + P(i, i) += mag_I_process_noise; } - // Don't continue to grow the body 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 - if (P.trace(State::mag_B.idx) < 0.1f) { + // mag_B: add process noise + float mag_B_sig = dt * math::constrain(_params.magb_p_noise, 0.f, 1.f); + float mag_B_process_noise = sq(mag_B_sig); - float mag_B_sig = dt * math::constrain(_params.magb_p_noise, 0.f, 1.f); - float mag_B_process_noise = sq(mag_B_sig); - - for (unsigned index = 0; index < State::mag_B.dof; index++) { - unsigned i = State::mag_B.idx + index; - P(i, i) += mag_B_process_noise; - } + for (unsigned index = 0; index < State::mag_B.dof; index++) { + const unsigned i = State::mag_B.idx + index; + P(i, i) += mag_B_process_noise; } } #endif // CONFIG_EKF2_MAGNETOMETER + #if defined(CONFIG_EKF2_WIND) if (_control_status.flags.wind) { - // 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 (P.trace(State::wind_vel.idx) < sq(_params.initial_wind_uncertainty)) { + // wind vel: add process noise + float wind_vel_nsd_scaled = math::constrain(_params.wind_vel_nsd, 0.f, 1.f) * (1.f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf)); + float wind_vel_process_noise = sq(wind_vel_nsd_scaled) * dt; - float wind_vel_nsd_scaled = math::constrain(_params.wind_vel_nsd, 0.f, 1.f) * (1.f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf)); - - 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; - P(i, i) += wind_vel_process_noise; - } + for (unsigned index = 0; index < State::wind_vel.dof; index++) { + const unsigned i = State::wind_vel.idx + index; + P(i, i) += wind_vel_process_noise; } } #endif // CONFIG_EKF2_WIND + // covariance matrix is symmetrical, so copy upper half to lower half for (unsigned row = 0; row < State::size; row++) { - for (unsigned column = 0 ; column < row; column++) { + for (unsigned column = 0; column < row; column++) { P(row, column) = P(column, row); } } - // fix gross errors in the covariance matrix and ensure rows and - // columns for un-used states are zero - fixCovarianceErrors(false); + constrainStateVariances(); } -void Ekf::fixCovarianceErrors(bool force_symmetry) +void Ekf::constrainStateVariances() { // NOTE: This limiting is a last resort and should not be relied on // TODO: Split covariance prediction into separate F*P*transpose(F) and Q contributions // 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) - 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; - - 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); - - // the following states are optional and are deactivated when not required - // by ensuring the corresponding covariance matrix values are kept at zero - - // 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 / sq(_dt_ekf_avg); - float maxStateVar = minSafeStateVar; - bool resetRequired = false; - - 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; - } - - if (P(stateIndex, stateIndex) > maxStateVar) { - maxStateVar = P(stateIndex, stateIndex); - - } else if (P(stateIndex, stateIndex) < minSafeStateVar) { - resetRequired = true; - } - } - - // 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 / sq(_dt_ekf_avg); - float minAllowedStateVar = fmaxf(0.01f * maxStateVar, minStateVarTarget); - - 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)); - } - - // If any one axis has fallen below the safe minimum, all delta velocity covariance terms must be reset to zero - if (resetRequired) { - resetAccelBiasCov(); - } - } - - if (force_symmetry) { - P.makeRowColSymmetric(State::quat_nominal.idx); - P.makeRowColSymmetric(State::vel.idx); - P.makeRowColSymmetric(State::pos.idx); - P.makeRowColSymmetric(State::gyro_bias.idx); - P.makeRowColSymmetric(State::accel_bias.idx); - } + constrainStateVar(State::quat_nominal, 1e-9f, 1.f); + constrainStateVar(State::vel, 1e-6f, 1e6f); + constrainStateVar(State::pos, 1e-6f, 1e6f); + constrainStateVarLimitRatio(State::gyro_bias, kGyroBiasVarianceMin, 1.f); + constrainStateVarLimitRatio(State::accel_bias, kAccelBiasVarianceMin, 1.f); #if defined(CONFIG_EKF2_MAGNETOMETER) - // magnetic field states - if (!_control_status.flags.mag) { - P.uncorrelateCovarianceSetVariance(State::mag_I.idx, 0.0f); - P.uncorrelateCovarianceSetVariance(State::mag_B.idx, 0.0f); - - } else { - const float mag_I_var_max = 1.f; - constrainStateVar(State::mag_I, 0.f, mag_I_var_max); - - const float mag_B_var_max = 1.f; - constrainStateVar(State::mag_B, 0.f, mag_B_var_max); - - if (force_symmetry) { - P.makeRowColSymmetric(State::mag_I.idx); - P.makeRowColSymmetric(State::mag_B.idx); - } + if (_control_status.flags.mag) { + constrainStateVarLimitRatio(State::mag_I, kMagVarianceMin, 1.f); + constrainStateVarLimitRatio(State::mag_B, kMagVarianceMin, 1.f); } #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_WIND) - // wind velocity states - if (!_control_status.flags.wind) { - P.uncorrelateCovarianceSetVariance(State::wind_vel.idx, 0.0f); - - } else { - const float wind_vel_var_max = 1e6f; - constrainStateVar(State::wind_vel, 0.f, wind_vel_var_max); - - if (force_symmetry) { - P.makeRowColSymmetric(State::wind_vel.idx); - } + if (_control_status.flags.wind) { + constrainStateVarLimitRatio(State::wind_vel, 1e-6f, 1e6f); } #endif // CONFIG_EKF2_WIND } @@ -336,20 +244,23 @@ void Ekf::constrainStateVar(const IdxDof &state, float min, float 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 SquareMatrixState &KHP) +void Ekf::constrainStateVarLimitRatio(const IdxDof &state, float min, float max, float max_ratio) { - bool healthy = true; + // the ratio of a max and min variance must not exceed max_ratio + float state_var_max = 0.f; - for (int i = 0; i < State::size; i++) { - if (P(i, i) < KHP(i, i)) { - P.uncorrelateCovarianceSetVariance<1>(i, 0.0f); - healthy = false; + for (unsigned i = state.idx; i < (state.idx + state.dof); i++) { + if (P(i, i) > state_var_max) { + state_var_max = P(i, i); } } - return healthy; + float limited_max = math::constrain(state_var_max, min, max); + float limited_min = math::constrain(limited_max / max_ratio, min, max); + + for (unsigned i = state.idx; i < (state.idx + state.dof); i++) { + P(i, i) = math::constrain(P(i, i), limited_min, limited_max); + } } void Ekf::resetQuatCov(const float yaw_noise) @@ -382,14 +293,10 @@ void Ekf::resetMagCov() P.uncorrelateCovarianceSetVariance(State::mag_I.idx, sq(_params.mag_noise)); P.uncorrelateCovarianceSetVariance(State::mag_B.idx, sq(_params.mag_noise)); - - saveMagCovData(); } #endif // CONFIG_EKF2_MAGNETOMETER void Ekf::resetGyroBiasZCov() { - const float init_gyro_bias_var = sq(_params.switch_on_gyro_bias); - - P.uncorrelateCovarianceSetVariance<1>(State::gyro_bias.idx + 2, init_gyro_bias_var); + P.uncorrelateCovarianceSetVariance<1>(State::gyro_bias.idx + 2, sq(_params.switch_on_gyro_bias)); } diff --git a/src/modules/ekf2/EKF/drag_fusion.cpp b/src/modules/ekf2/EKF/drag_fusion.cpp index 287bfb69f9e8..5b4baedf123f 100644 --- a/src/modules/ekf2/EKF/drag_fusion.cpp +++ b/src/modules/ekf2/EKF/drag_fusion.cpp @@ -37,8 +37,8 @@ */ #include "ekf.h" -#include -#include +#include +#include #include #include @@ -110,7 +110,7 @@ void Ekf::fuseDrag(const dragSample &drag_sample) bool fused[] {false, false}; - VectorState Kfusion; + VectorState H; // perform sequential fusion of XY specific forces for (uint8_t axis_index = 0; axis_index < 2; axis_index++) { @@ -128,16 +128,16 @@ void Ekf::fuseDrag(const dragSample &drag_sample) _aid_src_drag.innovation_variance[axis_index] = NAN; // reset if (axis_index == 0) { - sym::ComputeDragXInnovVarAndK(state_vector_prev, P, rho, bcoef_inv(axis_index), mcoef_corrrected, R_ACC, FLT_EPSILON, - &_aid_src_drag.innovation_variance[axis_index], &Kfusion); + sym::ComputeDragXInnovVarAndH(state_vector_prev, P, rho, bcoef_inv(axis_index), mcoef_corrrected, R_ACC, FLT_EPSILON, + &_aid_src_drag.innovation_variance[axis_index], &H); if (!using_bcoef_x && !using_mcoef) { continue; } } else if (axis_index == 1) { - sym::ComputeDragYInnovVarAndK(state_vector_prev, P, rho, bcoef_inv(axis_index), mcoef_corrrected, R_ACC, FLT_EPSILON, - &_aid_src_drag.innovation_variance[axis_index], &Kfusion); + sym::ComputeDragYInnovVarAndH(state_vector_prev, P, rho, bcoef_inv(axis_index), mcoef_corrrected, R_ACC, FLT_EPSILON, + &_aid_src_drag.innovation_variance[axis_index], &H); if (!using_bcoef_y && !using_mcoef) { continue; @@ -157,7 +157,10 @@ void Ekf::fuseDrag(const dragSample &drag_sample) && PX4_ISFINITE(_aid_src_drag.innovation_variance[axis_index]) && PX4_ISFINITE(_aid_src_drag.innovation[axis_index]) && (_aid_src_drag.test_ratio[axis_index] < 1.f) ) { - if (measurementUpdate(Kfusion, _aid_src_drag.innovation_variance[axis_index], _aid_src_drag.innovation[axis_index])) { + + VectorState K = P * H / _aid_src_drag.innovation_variance[axis_index]; + + if (measurementUpdate(K, H, R_ACC, _aid_src_drag.innovation[axis_index])) { fused[axis_index] = true; } } diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index f3330fed8cb2..b6de89f544bb 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -89,9 +89,6 @@ void Ekf::reset() _fault_status.value = 0; _innov_check_fail_status.value = 0; - _prev_gyro_bias_var.zero(); - _prev_accel_bias_var.zero(); - #if defined(CONFIG_EKF2_GNSS) resetGpsDriftCheckFilters(); _gps_checks_passed = false; @@ -312,8 +309,9 @@ void Ekf::predictState(const imuSample &imu_delayed) // predict position states via trapezoidal integration of velocity _state.pos += (vel_last + _state.vel) * imu_delayed.delta_vel_dt * 0.5f; - constrainStates(); - + // constrain states + _state.vel = matrix::constrain(_state.vel, -1000.f, 1000.f); + _state.pos = matrix::constrain(_state.pos, -1.e6f, 1.e6f); // some calculations elsewhere in code require a raw angular rate vector so calculate here to avoid duplication // protect against possible small timesteps resulting from timing slip on previous frame that can drive spikes into the rate @@ -328,22 +326,148 @@ void Ekf::predictState(const imuSample &imu_delayed) const float alpha = 1.0f - imu_delayed.delta_vel_dt; _accel_lpf_NE = _accel_lpf_NE * alpha + corrected_delta_vel_ef.xy(); - // calculate a yaw change about the earth frame vertical - const float spin_del_ang_D = corrected_delta_ang.dot(Vector3f(_R_to_earth.row(2))); - _yaw_delta_ef += spin_del_ang_D; - - // Calculate filtered yaw rate to be used by the magnetometer fusion type selection logic - // Note fixed coefficients are used to save operations. The exact time constant is not important. - _yaw_rate_lpf_ef = 0.95f * _yaw_rate_lpf_ef + 0.05f * spin_del_ang_D / imu_delayed.delta_ang_dt; - // Calculate low pass filtered height rate float alpha_height_rate_lpf = 0.1f * imu_delayed.delta_vel_dt; // 10 seconds time constant _height_rate_lpf = _height_rate_lpf * (1.0f - alpha_height_rate_lpf) + _state.vel(2) * alpha_height_rate_lpf; } +void Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation) +{ + + if (!_pos_ref.isInitialized()) { + return; + } + + // apply a first order correction using velocity at the delated time horizon and the delta time + timestamp_observation = math::min(_time_latest_us, timestamp_observation); + const float dt = _time_delayed_us > timestamp_observation ? static_cast(_time_delayed_us - timestamp_observation) + * 1e-6f : -static_cast(timestamp_observation - _time_delayed_us) * 1e-6f; + + Vector2f pos_corrected = _pos_ref.project(lat_deg, lon_deg) + _state.vel.xy() * dt; + + resetHorizontalPositionToExternal(pos_corrected, math::max(accuracy, FLT_EPSILON)); +} + void Ekf::updateParameters() { #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME) _aux_global_position.updateParameters(); #endif // CONFIG_EKF2_AUX_GLOBAL_POSITION } + +template +static void printRingBuffer(const char *name, RingBuffer *rb) +{ + if (rb) { + printf("%s: %d/%d entries (%d/%d Bytes) (%zu Bytes per entry)\n", + name, + rb->entries(), rb->get_length(), rb->get_used_size(), rb->get_total_size(), + sizeof(T)); + } +} + +void Ekf::print_status() +{ + printf("\nStates: (%.4f seconds ago)\n", (_time_latest_us - _time_delayed_us) * 1e-6); + printf("Orientation (%d-%d): [%.3f, %.3f, %.3f, %.3f] (Euler [%.1f, %.1f, %.1f] deg) var: [%.1e, %.1e, %.1e]\n", + State::quat_nominal.idx, State::quat_nominal.idx + State::quat_nominal.dof - 1, + (double)_state.quat_nominal(0), (double)_state.quat_nominal(1), (double)_state.quat_nominal(2), (double)_state.quat_nominal(3), + (double)math::degrees(matrix::Eulerf(_state.quat_nominal).phi()), (double)math::degrees(matrix::Eulerf(_state.quat_nominal).theta()), (double)math::degrees(matrix::Eulerf(_state.quat_nominal).psi()), + (double)getStateVariance()(0), (double)getStateVariance()(1), (double)getStateVariance()(2) + ); + + printf("Velocity (%d-%d): [%.3f, %.3f, %.3f] var: [%.1e, %.1e, %.1e]\n", + State::vel.idx, State::vel.idx + State::vel.dof - 1, + (double)_state.vel(0), (double)_state.vel(1), (double)_state.vel(2), + (double)getStateVariance()(0), (double)getStateVariance()(1), (double)getStateVariance()(2) + ); + + printf("Position (%d-%d): [%.3f, %.3f, %.3f] var: [%.1e, %.1e, %.1e]\n", + State::pos.idx, State::pos.idx + State::pos.dof - 1, + (double)_state.pos(0), (double)_state.pos(1), (double)_state.pos(2), + (double)getStateVariance()(0), (double)getStateVariance()(1), (double)getStateVariance()(2) + ); + + printf("Gyro Bias (%d-%d): [%.6f, %.6f, %.6f] var: [%.1e, %.1e, %.1e]\n", + State::gyro_bias.idx, State::gyro_bias.idx + State::gyro_bias.dof - 1, + (double)_state.gyro_bias(0), (double)_state.gyro_bias(1), (double)_state.gyro_bias(2), + (double)getStateVariance()(0), (double)getStateVariance()(1), (double)getStateVariance()(2) + ); + + printf("Accel Bias (%d-%d): [%.6f, %.6f, %.6f] var: [%.1e, %.1e, %.1e]\n", + State::accel_bias.idx, State::accel_bias.idx + State::accel_bias.dof - 1, + (double)_state.accel_bias(0), (double)_state.accel_bias(1), (double)_state.accel_bias(2), + (double)getStateVariance()(0), (double)getStateVariance()(1), (double)getStateVariance()(2) + ); + +#if defined(CONFIG_EKF2_MAGNETOMETER) + printf("Magnetic Field (%d-%d): [%.3f, %.3f, %.3f] var: [%.1e, %.1e, %.1e]\n", + State::mag_I.idx, State::mag_I.idx + State::mag_I.dof - 1, + (double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2), + (double)getStateVariance()(0), (double)getStateVariance()(1), (double)getStateVariance()(2) + ); + + printf("Magnetic Bias (%d-%d): [%.3f, %.3f, %.3f] var: [%.1e, %.1e, %.1e]\n", + State::mag_B.idx, State::mag_B.idx + State::mag_B.dof - 1, + (double)_state.mag_B(0), (double)_state.mag_B(1), (double)_state.mag_B(2), + (double)getStateVariance()(0), (double)getStateVariance()(1), + (double)getStateVariance()(2) + ); +#endif // CONFIG_EKF2_MAGNETOMETER + +#if defined(CONFIG_EKF2_WIND) + printf("Wind velocity (%d-%d): [%.3f, %.3f] var: [%.1e, %.1e]\n", + State::wind_vel.idx, State::wind_vel.idx + State::wind_vel.dof - 1, + (double)_state.wind_vel(0), (double)_state.wind_vel(1), + (double)getStateVariance()(0), (double)getStateVariance()(1) + ); +#endif // CONFIG_EKF2_WIND + + printf("\nP:\n"); + P.print(); + + printf("EKF average dt: %.6f seconds\n", (double)_dt_ekf_avg); + printf("minimum observation interval %d us\n", _min_obs_interval_us); + + printRingBuffer("IMU buffer", &_imu_buffer); + printRingBuffer("system flag buffer", _system_flag_buffer); + +#if defined(CONFIG_EKF2_AIRSPEED) + printRingBuffer("airspeed buffer", _airspeed_buffer); +#endif // CONFIG_EKF2_AIRSPEED + +#if defined(CONFIG_EKF2_AUXVEL) + printRingBuffer("aux vel buffer", _auxvel_buffer); +#endif // CONFIG_EKF2_AUXVEL + +#if defined(CONFIG_EKF2_BAROMETER) + printRingBuffer("baro buffer", _baro_buffer); +#endif // CONFIG_EKF2_BAROMETER + +#if defined(CONFIG_EKF2_DRAG_FUSION) + printRingBuffer("drag buffer", _drag_buffer); +#endif // CONFIG_EKF2_DRAG_FUSION + +#if defined(CONFIG_EKF2_EXTERNAL_VISION) + printRingBuffer("ext vision buffer", _ext_vision_buffer); +#endif // CONFIG_EKF2_EXTERNAL_VISION + +#if defined(CONFIG_EKF2_GNSS) + printRingBuffer("gps buffer", _gps_buffer); +#endif // CONFIG_EKF2_GNSS + +#if defined(CONFIG_EKF2_MAGNETOMETER) + printRingBuffer("mag buffer", _mag_buffer); +#endif // CONFIG_EKF2_MAGNETOMETER + +#if defined(CONFIG_EKF2_OPTICAL_FLOW) + printRingBuffer("flow buffer", _flow_buffer); +#endif // CONFIG_EKF2_OPTICAL_FLOW + +#if defined(CONFIG_EKF2_RANGE_FINDER) + printRingBuffer("range buffer", _range_buffer); +#endif // CONFIG_EKF2_RANGE_FINDER + + + _output_predictor.print_status(); +} diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index cff0426ada00..bc087558e9eb 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -85,6 +85,8 @@ class Ekf final : public EstimatorInterface // initialise variables to sane values (also interface class) bool init(uint64_t timestamp) override; + void print_status(); + // should be called every time new data is pushed into the filter bool update(); @@ -253,7 +255,12 @@ class Ekf final : public EstimatorInterface matrix::Vector covariances_diagonal() const { return P.diag(); } matrix::Vector getQuaternionVariance() const { return getStateVariance(); } + matrix::Vector3f getRotVarNed() const; + float getYawVar() const; + float getTiltVariance() const; + Vector3f getVelocityVariance() const { return getStateVariance(); }; + Vector3f getPositionVariance() const { return getStateVariance(); } // get the ekf WGS-84 origin position and height and the system time it was last set @@ -320,8 +327,8 @@ class Ekf final : public EstimatorInterface #endif } - // fuse single velocity and position measurement - bool fuseVelPosHeight(const float innov, const float innov_var, const int state_index); + // fuse single direct state measurement (eg NED velocity, NED position, mag earth field, etc) + bool fuseDirectStateMeasurement(const float innov, const float innov_var, const float R, const int state_index); // gyro bias const Vector3f &getGyroBias() const { return _state.gyro_bias; } // get the gyroscope bias in rad/s @@ -338,14 +345,7 @@ class Ekf final : public EstimatorInterface const Vector3f &getMagEarthField() const { return _state.mag_I; } const Vector3f &getMagBias() const { return _state.mag_B; } - Vector3f getMagBiasVariance() const - { - if (_control_status.flags.mag) { - return getStateVariance(); - } - - return _saved_mag_bf_covmat.diag(); - } + Vector3f getMagBiasVariance() const { return getStateVariance(); } // get the mag bias variance in Gauss float getMagBiasLimit() const { return 0.5f; } // 0.5 Gauss #endif // CONFIG_EKF2_MAGNETOMETER @@ -405,8 +405,6 @@ class Ekf final : public EstimatorInterface // return a bitmask integer that describes which state estimates can be used for flight control void get_ekf_soln_status(uint16_t *status) const; - float getYawVar() const; - HeightSensor getHeightSensorRef() const { return _height_sensor_ref; } #if defined(CONFIG_EKF2_AIRSPEED) @@ -470,36 +468,58 @@ class Ekf final : public EstimatorInterface const auto &aid_src_aux_vel() const { return _aid_src_aux_vel; } #endif // CONFIG_EKF2_AUXVEL - bool measurementUpdate(VectorState &K, float innovation_variance, float innovation) + bool measurementUpdate(VectorState &K, const VectorState &H, const float R, const float innovation) { clearInhibitedStateKalmanGains(K); - const VectorState KS = K * innovation_variance; - SquareMatrixState KHP; +#if false + // Matrix implementation of the Joseph stabilized covariance update + // This is extremely expensive to compute. Use for debugging purposes only. + auto A = matrix::eye(); + A -= K.multiplyByTranspose(H); + P = A * P; + P = P.multiplyByTranspose(A); - 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); + const VectorState KR = K * R; + P += KR.multiplyByTranspose(K); +#else + // Efficient implementation of the Joseph stabilized covariance update + // Based on "G. J. Bierman. Factorization Methods for Discrete Sequential Estimation. Academic Press, Dover Publications, New York, 1977, 2006" + // P = (I - K * H) * P * (I - K * H).T + K * R * K.T + // = P_temp * (I - H.T * K.T) + K * R * K.T + // = P_temp - P_temp * H.T * K.T + K * R * K.T + + // Step 1: conventional update + // Compute P_temp and store it in P to avoid allocating more memory + // P is symmetric, so PH == H.T * P.T == H.T * P. Taking the row is faster as matrices are row-major + VectorState PH = P * H; // H is stored as a column vector. H is in fact H.T + + for (unsigned i = 0; i < State::size; i++) { + for (unsigned j = 0; j < State::size; j++) { + P(i, j) -= K(i) * PH(j); // P is now not symmetrical if K is not optimal (e.g.: some gains have been zeroed) } } - const bool is_healthy = checkAndFixCovarianceUpdate(KHP); + // Step 2: stabilized update + PH = P * H; // H is stored as a column vector. H is in fact H.T - if (is_healthy) { - // apply the covariance corrections - P -= KHP; - - fixCovarianceErrors(true); - - // apply the state corrections - fuse(K, innovation); + for (unsigned i = 0; i < State::size; i++) { + for (unsigned j = 0; j <= i; j++) { + P(i, j) = P(i, j) - PH(i) * K(j) + K(i) * R * K(j); + P(j, i) = P(i, j); + } } +#endif + + constrainStateVariances(); - return is_healthy; + // apply the state corrections + fuse(K, innovation); + return true; } + void resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation); + void updateParameters(); friend class AuxGlobalPosition; @@ -516,6 +536,14 @@ class Ekf final : public EstimatorInterface void updateHorizontalDeadReckoningstatus(); void updateVerticalDeadReckoningStatus(); + static constexpr float kGyroBiasVarianceMin{1e-9f}; + static constexpr float kAccelBiasVarianceMin{1e-9f}; + +#if defined(CONFIG_EKF2_MAGNETOMETER) + static constexpr float kMagVarianceMin = 1e-6f; +#endif // CONFIG_EKF2_MAGNETOMETER + + struct StateResetCounts { uint8_t velNE{0}; ///< number of horizontal position reset events (allow to wrap if count exceeds 255) uint8_t velD{0}; ///< number of vertical velocity reset events (allow to wrap if count exceeds 255) @@ -563,8 +591,6 @@ class Ekf final : public EstimatorInterface Vector2f _accel_lpf_NE{}; ///< Low pass filtered horizontal earth frame acceleration (m/sec**2) float _height_rate_lpf{0.0f}; - 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) SquareMatrixState P{}; ///< state covariance matrix @@ -644,7 +670,6 @@ class Ekf final : public EstimatorInterface float _gps_velD_diff_filt{0.0f}; ///< GPS filtered Down velocity (m/sec) uint64_t _last_gps_fail_us{0}; ///< last system time in usec that the GPS failed it's checks uint64_t _last_gps_pass_us{0}; ///< last system time in usec that the GPS passed it's checks - float _gps_error_norm{1.0f}; ///< normalised gps error uint32_t _min_gps_health_time_us{10000000}; ///< GPS is marked as healthy only after this amount of time bool _gps_checks_passed{false}; ///> true when all active GPS checks have passed @@ -694,9 +719,7 @@ class Ekf final : public EstimatorInterface 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. @@ -712,22 +735,15 @@ class Ekf final : public EstimatorInterface // 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) 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 // variables used to inhibit accel bias learning bool _accel_bias_inhibit[3] {}; ///< true when the accel bias learning is being inhibited for the specified axis bool _gyro_bias_inhibit[3] {}; ///< true when the gyro bias learning is being inhibited for the specified axis - 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_gyro_bias_var{}; ///< saved gyro XYZ bias variances - Vector3f _prev_accel_bias_var{}; ///< saved accel XYZ bias variances - // imu fault status uint64_t _time_bad_vert_accel{0}; ///< last time a bad vertical accel was detected (uSec) uint64_t _time_good_vert_accel{0}; ///< last time a good vertical accel was detected (uSec) @@ -782,6 +798,7 @@ class Ekf final : public EstimatorInterface // Reset the wind states using the current airspeed measurement, ground relative nav velocity, yaw angle and assumption of zero sideslip void resetWindUsingAirspeed(const airspeedSample &airspeed_sample); + void resetVelUsingAirspeed(const airspeedSample &airspeed_sample); #endif // CONFIG_EKF2_AIRSPEED #if defined(CONFIG_EKF2_SIDESLIP) @@ -810,6 +827,7 @@ class Ekf final : public EstimatorInterface void resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var); void resetHorizontalPositionToLastKnown(); + void resetHorizontalPositionToExternal(const Vector2f &new_horiz_pos, float horiz_accuracy); void resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f &new_horz_pos_var); void resetHorizontalPositionTo(const Vector2f &new_horz_pos, const float pos_var = NAN) { resetHorizontalPositionTo(new_horz_pos, Vector2f(pos_var, pos_var)); } @@ -825,7 +843,7 @@ class Ekf final : public EstimatorInterface void updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const float obs, const float obs_var, const float innov_gate, estimator_aid_source1d_s &aid_src) const; // 2d & 3d velocity aid source - void updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, const float innov_gate, estimator_aid_source2d_s &aid_src) const; + void updateHorizontalVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, const float innov_gate, estimator_aid_source2d_s &aid_src) const; void updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &obs, const Vector3f &obs_var, const float innov_gate, estimator_aid_source3d_s &aid_src) const; // horizontal and vertical position fusion @@ -833,7 +851,7 @@ class Ekf final : public EstimatorInterface void fuseVerticalPosition(estimator_aid_source1d_s &hgt_aid_src); // 2d & 3d velocity fusion - void fuseVelocity(estimator_aid_source2d_s &vel_aid_src); + void fuseHorizontalVelocity(estimator_aid_source2d_s &vel_aid_src); void fuseVelocity(estimator_aid_source3d_s &vel_aid_src); #if defined(CONFIG_EKF2_TERRAIN) @@ -939,18 +957,11 @@ class Ekf final : public EstimatorInterface #endif // CONFIG_EKF2_WIND } - // if the covariance correction will result in a negative variance, then - // the covariance matrix is unhealthy and must be corrected - 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 constrainStateVariances(); void constrainStateVar(const IdxDof &state, float min, float max); - - // constrain the ekf states - void constrainStates(); + void constrainStateVarLimitRatio(const IdxDof &state, float min, float max, float max_ratio = 1.e6f); // generic function which will perform a fusion step given a kalman gain K // and a scalar innovation value @@ -1047,7 +1058,6 @@ class Ekf final : public EstimatorInterface bool haglYawResetReq(); void checkYawAngleObservability(); - void checkMagBiasObservability(); void checkMagHeadingConsistency(); bool checkMagField(const Vector3f &mag); @@ -1056,10 +1066,6 @@ class Ekf final : public EstimatorInterface 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); @@ -1139,8 +1145,6 @@ class Ekf final : public EstimatorInterface void resetFakePosFusion(); void stopFakePosFusion(); - 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) // yaw_variance : yaw error variance (rad^2) diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 75544f4e3f38..bb9e901f5760 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -45,121 +45,6 @@ #include #include -void Ekf::resetHorizontalVelocityToZero() -{ - _information_events.flags.reset_vel_to_zero = true; - ECL_INFO("reset velocity to zero"); - // Used when falling back to non-aiding mode of operation - resetHorizontalVelocityTo(Vector2f{0.f, 0.f}, 25.f); -} - -void Ekf::resetVelocityTo(const Vector3f &new_vel, const Vector3f &new_vel_var) -{ - resetHorizontalVelocityTo(Vector2f(new_vel), Vector2f(new_vel_var(0), new_vel_var(1))); - resetVerticalVelocityTo(new_vel(2), new_vel_var(2)); -} - -void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f &new_horz_vel_var) -{ - const Vector2f delta_horz_vel = new_horz_vel - Vector2f(_state.vel); - _state.vel.xy() = new_horz_vel; - - if (PX4_ISFINITE(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>(State::vel.idx + 1, math::max(sq(0.01f), new_horz_vel_var(1))); - } - - _output_predictor.resetHorizontalVelocityTo(delta_horz_vel); - - // record the state change - if (_state_reset_status.reset_count.velNE == _state_reset_count_prev.velNE) { - _state_reset_status.velNE_change = delta_horz_vel; - - } else { - // there's already a reset this update, accumulate total delta - _state_reset_status.velNE_change += delta_horz_vel; - } - - _state_reset_status.reset_count.velNE++; - - // Reset the timout timer - _time_last_hor_vel_fuse = _time_delayed_us; -} - -void Ekf::resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var) -{ - const float delta_vert_vel = new_vert_vel - _state.vel(2); - _state.vel(2) = new_vert_vel; - - if (PX4_ISFINITE(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); - - // record the state change - if (_state_reset_status.reset_count.velD == _state_reset_count_prev.velD) { - _state_reset_status.velD_change = delta_vert_vel; - - } else { - // there's already a reset this update, accumulate total delta - _state_reset_status.velD_change += delta_vert_vel; - } - - _state_reset_status.reset_count.velD++; - - // Reset the timout timer - _time_last_ver_vel_fuse = _time_delayed_us; -} - -void Ekf::resetHorizontalPositionToLastKnown() -{ - ECL_INFO("reset position to last known (%.3f, %.3f)", (double)_last_known_pos(0), (double)_last_known_pos(1)); - - _information_events.flags.reset_pos_to_last_known = true; - - // Used when falling back to non-aiding mode of operation - resetHorizontalPositionTo(_last_known_pos.xy(), sq(_params.pos_noaid_noise)); -} - -void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f &new_horz_pos_var) -{ - const Vector2f delta_horz_pos{new_horz_pos - Vector2f{_state.pos}}; - _state.pos.xy() = new_horz_pos; - - if (PX4_ISFINITE(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>(State::pos.idx + 1, math::max(sq(0.01f), new_horz_pos_var(1))); - } - - _output_predictor.resetHorizontalPositionTo(delta_horz_pos); - - // record the state change - if (_state_reset_status.reset_count.posNE == _state_reset_count_prev.posNE) { - _state_reset_status.posNE_change = delta_horz_pos; - - } else { - // there's already a reset this update, accumulate total delta - _state_reset_status.posNE_change += delta_horz_pos; - } - - _state_reset_status.reset_count.posNE++; - -#if defined(CONFIG_EKF2_EXTERNAL_VISION) - _ev_pos_b_est.setBias(_ev_pos_b_est.getBias() - _state_reset_status.posNE_change); -#endif // CONFIG_EKF2_EXTERNAL_VISION - //_gps_pos_b_est.setBias(_gps_pos_b_est.getBias() + _state_reset_status.posNE_change); - - // Reset the timout timer - _time_last_hor_pos_fuse = _time_delayed_us; -} - bool Ekf::isHeightResetRequired() const { // check if height is continuously failing because of accel errors @@ -171,84 +56,6 @@ bool Ekf::isHeightResetRequired() const return (continuous_bad_accel_hgt || hgt_fusion_timeout); } -void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_var) -{ - const float old_vert_pos = _state.pos(2); - _state.pos(2) = new_vert_pos; - - if (PX4_ISFINITE(new_vert_pos_var)) { - // the state variance is the same as the observation - 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; - - // apply the change in height / height rate to our newest height / height rate estimate - // which have already been taken out from the output buffer - _output_predictor.resetVerticalPositionTo(new_vert_pos, delta_z); - - // record the state change - if (_state_reset_status.reset_count.posD == _state_reset_count_prev.posD) { - _state_reset_status.posD_change = delta_z; - - } else { - // there's already a reset this update, accumulate total delta - _state_reset_status.posD_change += delta_z; - } - - _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 -#if defined(CONFIG_EKF2_GNSS) - _gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + delta_z); -#endif // CONFIG_EKF2_GNSS -#if defined(CONFIG_EKF2_RANGE_FINDER) - _rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + delta_z); -#endif // CONFIG_EKF2_RANGE_FINDER - -#if defined(CONFIG_EKF2_TERRAIN) - terrainHandleVerticalPositionReset(delta_z); -#endif - - // Reset the timout timer - _time_last_hgt_fuse = _time_delayed_us; -} - -void Ekf::resetVerticalVelocityToZero() -{ - // we don't know what the vertical velocity is, so set it to zero - // Set the variance to a value large enough to allow the state to converge quickly - // that does not destabilise the filter - resetVerticalVelocityTo(0.0f, 10.f); -} - -void Ekf::constrainStates() -{ - _state.quat_nominal = matrix::constrain(_state.quat_nominal, -1.0f, 1.0f); - _state.vel = matrix::constrain(_state.vel, -1000.0f, 1000.0f); - _state.pos = matrix::constrain(_state.pos, -1.e6f, 1.e6f); - - const float gyro_bias_limit = getGyroBiasLimit(); - _state.gyro_bias = matrix::constrain(_state.gyro_bias, -gyro_bias_limit, gyro_bias_limit); - - const float accel_bias_limit = getAccelBiasLimit(); - _state.accel_bias = matrix::constrain(_state.accel_bias, -accel_bias_limit, accel_bias_limit); - -#if defined(CONFIG_EKF2_MAGNETOMETER) - _state.mag_I = matrix::constrain(_state.mag_I, -1.0f, 1.0f); - _state.mag_B = matrix::constrain(_state.mag_B, -getMagBiasLimit(), getMagBiasLimit()); -#endif // CONFIG_EKF2_MAGNETOMETER - -#if defined(CONFIG_EKF2_WIND) - _state.wind_vel = matrix::constrain(_state.wind_vel, -100.0f, 100.0f); -#endif // CONFIG_EKF2_WIND -} - #if defined(CONFIG_EKF2_BARO_COMPENSATION) float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated) const { @@ -378,36 +185,30 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons return false; } -// get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position 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.trace<2>(State::pos.idx) + sq(_gpos_origin_eph)); + float eph = INFINITY; + float epv = INFINITY; - // 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 - // and using state variances for accuracy reporting is overly optimistic in these situations - if (_control_status.flags.inertial_dead_reckoning) { -#if defined(CONFIG_EKF2_GNSS) - if (_control_status.flags.gps) { - hpos_err = math::max(hpos_err, Vector2f(_aid_src_gnss_pos.innovation).norm()); - } -#endif // CONFIG_EKF2_GNSS + if (global_origin_valid()) { + // report absolute accuracy taking into account the uncertainty in location of the origin + eph = sqrtf(P.trace<2>(State::pos.idx + 0) + sq(_gpos_origin_eph)); + epv = sqrtf(P.trace<1>(State::pos.idx + 2) + sq(_gpos_origin_epv)); -#if defined(CONFIG_EKF2_EXTERNAL_VISION) - if (_control_status.flags.ev_pos) { - hpos_err = math::max(hpos_err, Vector2f(_aid_src_ev_pos.innovation).norm()); + if (_horizontal_deadreckon_time_exceeded) { + float lpos_eph = 0.f; + float lpos_epv = 0.f; + get_ekf_lpos_accuracy(&lpos_eph, &lpos_epv); + + eph = math::max(eph, lpos_eph); + epv = math::max(epv, lpos_epv); } -#endif // CONFIG_EKF2_EXTERNAL_VISION } - *ekf_eph = hpos_err; - *ekf_epv = sqrtf(P(State::pos.idx + 2, State::pos.idx + 2) + sq(_gpos_origin_epv)); + *ekf_eph = eph; + *ekf_epv = 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 @@ -548,9 +349,6 @@ void Ekf::resetGyroBiasCov() // Zero the corresponding covariances and set // variances to the values use for initial alignment P.uncorrelateCovarianceSetVariance(State::gyro_bias.idx, sq(_params.switch_on_gyro_bias)); - - // Set previous frame values - _prev_gyro_bias_var = getStateVariance(); } void Ekf::resetAccelBias() @@ -566,9 +364,6 @@ void Ekf::resetAccelBiasCov() // Zero the corresponding covariances and set // variances to the values use for initial alignment P.uncorrelateCovarianceSetVariance(State::accel_bias.idx, sq(_params.switch_on_accel_bias)); - - // Set previous frame values - _prev_accel_bias_var = getStateVariance(); } // get EKF innovation consistency check status information comprising of: @@ -712,7 +507,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const { ekf_solution_status_u soln_status{}; // TODO: Is this accurate enough? - soln_status.flags.attitude = _control_status.flags.tilt_align && _control_status.flags.yaw_align && (_fault_status.value == 0); + soln_status.flags.attitude = attitude_valid(); soln_status.flags.velocity_horiz = (isHorizontalAidingActive() || (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd)) && (_fault_status.value == 0); soln_status.flags.velocity_vert = (_control_status.flags.baro_hgt || _control_status.flags.ev_hgt || _control_status.flags.gps_hgt || _control_status.flags.rng_hgt) && (_fault_status.value == 0); soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0); @@ -755,23 +550,39 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const void Ekf::fuse(const VectorState &K, float innovation) { + // quat_nominal Quatf delta_quat(matrix::AxisAnglef(K.slice(State::quat_nominal.idx, 0) * (-1.f * innovation))); _state.quat_nominal *= delta_quat; _state.quat_nominal.normalize(); _R_to_earth = Dcmf(_state.quat_nominal); - _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; + // vel + _state.vel = matrix::constrain(_state.vel - K.slice(State::vel.idx, 0) * innovation, -1.e3f, 1.e3f); + + // pos + _state.pos = matrix::constrain(_state.pos - K.slice(State::pos.idx, 0) * innovation, -1.e6f, 1.e6f); + + // gyro_bias + _state.gyro_bias = matrix::constrain(_state.gyro_bias - K.slice(State::gyro_bias.idx, 0) * innovation, + -getGyroBiasLimit(), getGyroBiasLimit()); + + // accel_bias + _state.accel_bias = matrix::constrain(_state.accel_bias - K.slice(State::accel_bias.idx, 0) * innovation, + -getAccelBiasLimit(), getAccelBiasLimit()); #if defined(CONFIG_EKF2_MAGNETOMETER) - _state.mag_I -= K.slice(State::mag_I.idx, 0) * innovation; - _state.mag_B -= K.slice(State::mag_B.idx, 0) * innovation; + // mag_I, mag_B + if (_control_status.flags.mag) { + _state.mag_I = matrix::constrain(_state.mag_I - K.slice(State::mag_I.idx, 0) * innovation, -1.f, 1.f); + _state.mag_B = matrix::constrain(_state.mag_B - K.slice(State::mag_B.idx, 0) * innovation, -getMagBiasLimit(), getMagBiasLimit()); + } #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_WIND) - _state.wind_vel -= K.slice(State::wind_vel.idx, 0) * innovation; + // wind_vel + if (_control_status.flags.wind) { + _state.wind_vel = matrix::constrain(_state.wind_vel - K.slice(State::wind_vel.idx, 0) * innovation, -1.e2f, 1.e2f); + } #endif // CONFIG_EKF2_WIND } @@ -849,11 +660,21 @@ void Ekf::updateVerticalDeadReckoningStatus() } } +Vector3f Ekf::getRotVarNed() const +{ + const matrix::SquareMatrix3f rot_cov_body = getStateCovariance(); + return matrix::SquareMatrix(_R_to_earth * rot_cov_body * _R_to_earth.T()).diag(); +} + float Ekf::getYawVar() const { - const matrix::SquareMatrix3f rot_cov = diag(getQuaternionVariance()); - const auto rot_var_ned = matrix::SquareMatrix(_R_to_earth * rot_cov * _R_to_earth.T()).diag(); - return rot_var_ned(2); + return getRotVarNed()(2); +} + +float Ekf::getTiltVariance() const +{ + const Vector3f rot_var_ned = getRotVarNed(); + return rot_var_ned(0) + rot_var_ned(1); } #if defined(CONFIG_EKF2_BAROMETER) @@ -887,8 +708,7 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance) 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(getQuaternionVariance()); - Vector3f rot_var_ned_before_reset = matrix::SquareMatrix3f(_R_to_earth * rot_cov * _R_to_earth.T()).diag(); + Vector3f rot_var_ned_before_reset = getRotVarNed(); // update the yaw angle variance if (PX4_ISFINITE(yaw_variance) && (yaw_variance > FLT_EPSILON)) { @@ -981,39 +801,19 @@ void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed) const float beta = 1.f - alpha; _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); + // gyro bias inhibit const bool do_inhibit_all_gyro_axes = !(_params.imu_ctrl & static_cast(ImuCtrl::GyroBias)); for (unsigned index = 0; index < State::gyro_bias.dof; index++) { - const unsigned stateIndex = State::gyro_bias.idx + index; - - bool is_bias_observable = true; - - // TODO: gyro bias conditions - - const bool do_inhibit_axis = do_inhibit_all_gyro_axes || !is_bias_observable; - - if (do_inhibit_axis) { - // store the bias state variances to be reinstated later - if (!_gyro_bias_inhibit[index]) { - _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_gyro_bias_var(index); - _gyro_bias_inhibit[index] = false; - } - } + bool is_bias_observable = true; // TODO: gyro bias conditions + _gyro_bias_inhibit[index] = do_inhibit_all_gyro_axes || !is_bias_observable; } // accel bias inhibit @@ -1022,8 +822,6 @@ void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed) || _fault_status.flags.bad_acc_vertical; for (unsigned index = 0; index < State::accel_bias.dof; index++) { - const unsigned stateIndex = State::accel_bias.idx + index; - bool is_bias_observable = true; if (_control_status.flags.vehicle_at_rest) { @@ -1037,22 +835,66 @@ void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed) is_bias_observable = (fabsf(_R_to_earth(2, index)) > 0.966f); // cos 15 degrees ~= 0.966 } - const bool do_inhibit_axis = do_inhibit_all_accel_axes || imu_delayed.delta_vel_clipping[index] || !is_bias_observable; + _accel_bias_inhibit[index] = do_inhibit_all_accel_axes || imu_delayed.delta_vel_clipping[index] || !is_bias_observable; + } +} + +bool Ekf::fuseDirectStateMeasurement(const float innov, const float innov_var, const float R, const int state_index) +{ + VectorState K; // Kalman gain vector for any single observation - sequential fusion is used. - if (do_inhibit_axis) { - // store the bias state variances to be reinstated later - if (!_accel_bias_inhibit[index]) { - _prev_accel_bias_var(index) = P(stateIndex, stateIndex); - _accel_bias_inhibit[index] = true; - } + // 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; + } - } else { - if (_accel_bias_inhibit[index]) { - // reinstate the bias state variances - P(stateIndex, stateIndex) = _prev_accel_bias_var(index); - _accel_bias_inhibit[index] = false; - } + clearInhibitedStateKalmanGains(K); + +#if false + // Matrix implementation of the Joseph stabilized covariance update + // This is extremely expensive to compute. Use for debugging purposes only. + auto A = matrix::eye(); + VectorState H; + H(state_index) = 1.f; + A -= K.multiplyByTranspose(H); + P = A * P; + P = P.multiplyByTranspose(A); + + const VectorState KR = K * R; + P += KR.multiplyByTranspose(K); +#else + // Efficient implementation of the Joseph stabilized covariance update + // Based on "G. J. Bierman. Factorization Methods for Discrete Sequential Estimation. Academic Press, Dover Publications, New York, 1977, 2006" + // P = (I - K * H) * P * (I - K * H).T + K * R * K.T + // = P_temp * (I - H.T * K.T) + K * R * K.T + // = P_temp - P_temp * H.T * K.T + K * R * K.T + + // Step 1: conventional update + // Compute P_temp and store it in P to avoid allocating more memory + // P is symmetric, so PH == H.T * P.T == H.T * P. Taking the row is faster as matrices are row-major + VectorState PH = P.row(state_index); + + for (unsigned i = 0; i < State::size; i++) { + for (unsigned j = 0; j < State::size; j++) { + P(i, j) -= K(i) * PH(j); // P is now not symmetric if K is not optimal (e.g.: some gains have been zeroed) } } + // Step 2: stabilized update + // P (or "P_temp") is not symmetric so we must take the column + PH = P.col(state_index); + + for (unsigned i = 0; i < State::size; i++) { + for (unsigned j = 0; j <= i; j++) { + P(i, j) = P(i, j) - PH(i) * K(j) + K(i) * R * K(j); + P(j, i) = P(i, j); + } + } +#endif + + constrainStateVariances(); + + // apply the state corrections + fuse(K, innov); + return true; } diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index a8a53f5a29bb..0b384bfb5649 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -676,62 +676,3 @@ void EstimatorInterface::printBufferAllocationFailed(const char *buffer_name) ECL_ERR("%s buffer allocation failed", buffer_name); } } - -void EstimatorInterface::print_status() -{ - printf("EKF average dt: %.6f seconds\n", (double)_dt_ekf_avg); - - printf("IMU buffer: %d (%d Bytes)\n", _imu_buffer.get_length(), _imu_buffer.get_total_size()); - - printf("minimum observation interval %d us\n", _min_obs_interval_us); - -#if defined(CONFIG_EKF2_GNSS) - if (_gps_buffer) { - printf("gps buffer: %d/%d (%d Bytes)\n", _gps_buffer->entries(), _gps_buffer->get_length(), _gps_buffer->get_total_size()); - } -#endif // CONFIG_EKF2_GNSS - -#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) { - printf("range buffer: %d/%d (%d Bytes)\n", _range_buffer->entries(), _range_buffer->get_length(), _range_buffer->get_total_size()); - } -#endif // CONFIG_EKF2_RANGE_FINDER - -#if defined(CONFIG_EKF2_AIRSPEED) - if (_airspeed_buffer) { - printf("airspeed buffer: %d/%d (%d Bytes)\n", _airspeed_buffer->entries(), _airspeed_buffer->get_length(), _airspeed_buffer->get_total_size()); - } -#endif // CONFIG_EKF2_AIRSPEED - -#if defined(CONFIG_EKF2_OPTICAL_FLOW) - if (_flow_buffer) { - printf("flow buffer: %d/%d (%d Bytes)\n", _flow_buffer->entries(), _flow_buffer->get_length(), _flow_buffer->get_total_size()); - } -#endif // CONFIG_EKF2_OPTICAL_FLOW - -#if defined(CONFIG_EKF2_EXTERNAL_VISION) - if (_ext_vision_buffer) { - printf("vision buffer: %d/%d (%d Bytes)\n", _ext_vision_buffer->entries(), _ext_vision_buffer->get_length(), _ext_vision_buffer->get_total_size()); - } -#endif // CONFIG_EKF2_EXTERNAL_VISION - -#if defined(CONFIG_EKF2_DRAG_FUSION) - if (_drag_buffer) { - printf("drag buffer: %d/%d (%d Bytes)\n", _drag_buffer->entries(), _drag_buffer->get_length(), _drag_buffer->get_total_size()); - } -#endif // CONFIG_EKF2_DRAG_FUSION - - _output_predictor.print_status(); -} diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 4fec3244eb80..e94d14b5e6cb 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -312,8 +312,6 @@ class EstimatorInterface const MapProjection &global_origin() const { return _pos_ref; } float getEkfGlobalOriginAltitude() const { return PX4_ISFINITE(_gps_alt_ref) ? _gps_alt_ref : 0.f; } - void print_status(); - OutputPredictor &output_predictor() { return _output_predictor; }; protected: @@ -479,7 +477,7 @@ class EstimatorInterface warning_event_status_u _warning_events{}; information_event_status_u _information_events{}; -private: + unsigned _min_obs_interval_us{0}; // minimum time interval between observations that will guarantee data is not lost (usec) #if defined(CONFIG_EKF2_DRAG_FUSION) void setDragData(const imuSample &imu); @@ -492,7 +490,5 @@ class EstimatorInterface void printBufferAllocationFailed(const char *buffer_name); ImuDownSampler _imu_down_sampler{_params.filter_update_interval_us}; - - unsigned _min_obs_interval_us{0}; // minimum time interval between observations that will guarantee data is not lost (usec) }; #endif // !EKF_ESTIMATOR_INTERFACE_H diff --git a/src/modules/ekf2/EKF/ev_control.cpp b/src/modules/ekf2/EKF/ev_control.cpp index 22ac4ec76474..0780f4111814 100644 --- a/src/modules/ekf2/EKF/ev_control.cpp +++ b/src/modules/ekf2/EKF/ev_control.cpp @@ -41,6 +41,7 @@ void Ekf::controlExternalVisionFusion() { _ev_pos_b_est.predict(_dt_ekf_avg); + _ev_hgt_b_est.predict(_dt_ekf_avg); // Check for new external vision data extVisionSample ev_sample; diff --git a/src/modules/ekf2/EKF/ev_height_control.cpp b/src/modules/ekf2/EKF/ev_height_control.cpp index c0980a71b3c4..0552ace53c76 100644 --- a/src/modules/ekf2/EKF/ev_height_control.cpp +++ b/src/modules/ekf2/EKF/ev_height_control.cpp @@ -45,7 +45,7 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com HeightBiasEstimator &bias_est = _ev_hgt_b_est; - bias_est.predict(_dt_ekf_avg); + // bias_est.predict(_dt_ekf_avg) called by controlExternalVisionFusion() // correct position for offset relative to IMU const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body; diff --git a/src/modules/ekf2/EKF/fake_pos_control.cpp b/src/modules/ekf2/EKF/fake_pos_control.cpp index 4962cd55fc41..cb3d246f235b 100644 --- a/src/modules/ekf2/EKF/fake_pos_control.cpp +++ b/src/modules/ekf2/EKF/fake_pos_control.cpp @@ -43,7 +43,7 @@ void Ekf::controlFakePosFusion() auto &aid_src = _aid_src_fake_pos; // If we aren't doing any aiding, fake position measurements at the last known position to constrain drift - // During intial tilt aligment, fake position is used to perform a "quasi-stationary" leveling of the EKF + // During initial tilt alignment, fake position is used to perform a "quasi-stationary" leveling of the EKF const bool fake_pos_data_ready = !isHorizontalAidingActive() && isTimedOut(aid_src.time_last_fuse, (uint64_t)2e5); // Fuse fake position at a limited rate @@ -68,7 +68,10 @@ void Ekf::controlFakePosFusion() updateHorizontalPositionAidSrcStatus(_time_delayed_us, Vector2f(_last_known_pos), obs_var, innov_gate, aid_src); - const bool continuing_conditions_passing = !isHorizontalAidingActive(); + const bool continuing_conditions_passing = !isHorizontalAidingActive() + && ((getTiltVariance() > sq(math::radians(3.f))) || _control_status.flags.vehicle_at_rest) + && (!(_params.imu_ctrl & static_cast(ImuCtrl::GravityVector)) || _control_status.flags.vehicle_at_rest); + const bool starting_conditions_passing = continuing_conditions_passing && _horizontal_deadreckon_time_exceeded; @@ -77,8 +80,8 @@ void Ekf::controlFakePosFusion() // always protect against extreme values that could result in a NaN if ((aid_src.test_ratio[0] < sq(100.0f / innov_gate)) - && (aid_src.test_ratio[1] < sq(100.0f / innov_gate)) - ) { + && (aid_src.test_ratio[1] < sq(100.0f / innov_gate)) + ) { fuseHorizontalPosition(aid_src); } diff --git a/src/modules/ekf2/EKF/gps_checks.cpp b/src/modules/ekf2/EKF/gps_checks.cpp index d4414f8a518e..71018df1c708 100644 --- a/src/modules/ekf2/EKF/gps_checks.cpp +++ b/src/modules/ekf2/EKF/gps_checks.cpp @@ -155,10 +155,6 @@ bool Ekf::runGnssChecks(const gnssSample &gps) // Check the reported speed accuracy _gps_check_fail_status.flags.sacc = (gps.sacc > _params.req_sacc); - // check if GPS quality is degraded - _gps_error_norm = fmaxf((gps.hacc / _params.req_hacc), (gps.vacc / _params.req_vacc)); - _gps_error_norm = fmaxf(_gps_error_norm, (gps.sacc / _params.req_sacc)); - // Calculate time lapsed since last update, limit to prevent numerical errors and calculate a lowpass filter coefficient constexpr float filt_time_const = 10.0f; const float dt = math::constrain(float(int64_t(gps.time_us) - int64_t(_gps_pos_prev.getProjectionReferenceTimestamp())) * 1e-6f, 0.001f, filt_time_const); diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp index 83670b5655d6..03cf0a326f28 100644 --- a/src/modules/ekf2/EKF/gps_control.cpp +++ b/src/modules/ekf2/EKF/gps_control.cpp @@ -47,7 +47,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) } if (!gyro_bias_inhibited()) { - _yawEstimator.setGyroBias(getGyroBias()); + _yawEstimator.setGyroBias(getGyroBias(), _control_status.flags.vehicle_at_rest); } // run EKF-GSF yaw estimator once per imu_delayed update @@ -80,8 +80,11 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) } } + if (_pos_ref.isInitialized()) { + updateGnssPos(gnss_sample, _aid_src_gnss_pos); + } + updateGnssVel(gnss_sample, _aid_src_gnss_vel); - updateGnssPos(gnss_sample, _aid_src_gnss_pos); } else if (_control_status.flags.gps) { if (!isNewestSampleRecent(_time_last_gps_buffer_push, _params.reset_timeout_max)) { @@ -243,7 +246,7 @@ bool Ekf::tryYawEmergencyReset() bool success = false; /* A rapid reset to the yaw emergency estimate is performed if horizontal velocity innovation checks continuously - * fails while the difference between the yaw emergency estimator and the yas estimate is large. + * fails while the difference between the yaw emergency estimator and the yaw estimate is large. * This enables recovery from a bad yaw estimate. A reset is not performed if the fault condition was * present before flight to prevent triggering due to GPS glitches or other sensor errors. */ diff --git a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp index 63e54fdc9ccc..13c057f26653 100644 --- a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp +++ b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp @@ -134,7 +134,7 @@ void Ekf::fuseGpsYaw(float antenna_yaw_offset) // only calculate gains for states we are using VectorState Kfusion = P * H / gnss_yaw.innovation_variance; - const bool is_fused = measurementUpdate(Kfusion, gnss_yaw.innovation_variance, gnss_yaw.innovation); + const bool is_fused = measurementUpdate(Kfusion, H, gnss_yaw.observation_variance, gnss_yaw.innovation); _fault_status.flags.bad_hdg = !is_fused; gnss_yaw.fused = is_fused; diff --git a/src/modules/ekf2/EKF/gravity_fusion.cpp b/src/modules/ekf2/EKF/gravity_fusion.cpp index 13ae1112c6bf..70a0a8ded200 100644 --- a/src/modules/ekf2/EKF/gravity_fusion.cpp +++ b/src/modules/ekf2/EKF/gravity_fusion.cpp @@ -40,30 +40,33 @@ */ #include "ekf.h" -#include +#include +#include +#include #include void Ekf::controlGravityFusion(const imuSample &imu) { - // fuse gravity observation if our overall acceleration isn't too big - const float gravity_scale = _accel_vec_filt.norm() / CONSTANTS_ONE_G; + // get raw accelerometer reading at delayed horizon and expected measurement noise (gaussian) + const Vector3f measurement = Vector3f(imu.delta_vel / imu.delta_vel_dt - _state.accel_bias).unit(); + const float measurement_var = math::max(sq(_params.gravity_noise), sq(0.01f)); + const float upper_accel_limit = CONSTANTS_ONE_G * 1.1f; + const float lower_accel_limit = CONSTANTS_ONE_G * 0.9f; + const bool accel_lpf_norm_good = (_accel_magnitude_filt > lower_accel_limit) && (_accel_magnitude_filt < upper_accel_limit); + + // fuse gravity observation if our overall acceleration isn't too big _control_status.flags.gravity_vector = (_params.imu_ctrl & static_cast(ImuCtrl::GravityVector)) - && (((gravity_scale >= 0.9f && gravity_scale <= 1.1f)) || _control_status.flags.vehicle_at_rest) + && (accel_lpf_norm_good || _control_status.flags.vehicle_at_rest) && !isHorizontalAidingActive(); - // get raw accelerometer reading at delayed horizon and expected measurement noise (gaussian) - const Vector3f measurement = imu.delta_vel / imu.delta_vel_dt - getAccelBias(); - const float measurement_var = sq(_params.gravity_noise); - // calculate kalman gains and innovation variances - Vector3f innovation; // innovation of the last gravity fusion observation (m/s**2) + Vector3f innovation = _state.quat_nominal.rotateVectorInverse(Vector3f(0.f, 0.f, -1.f)) - measurement; Vector3f innovation_variance; - VectorState Kx, Ky, Kz; // Kalman gain vectors - sym::ComputeGravityInnovVarAndKAndH( - _state.vector(), P, measurement, measurement_var, FLT_EPSILON, - &innovation, &innovation_variance, &Kx, &Ky, &Kz); + const auto state_vector = _state.vector(); + VectorState H; + sym::ComputeGravityXyzInnovVarAndHx(state_vector, P, measurement_var, &innovation_variance, &H); // fill estimator aid source status resetEstimatorAidStatus(_aid_src_gravity); @@ -77,19 +80,43 @@ void Ekf::controlGravityFusion(const imuSample &imu) innovation.copyTo(_aid_src_gravity.innovation); innovation_variance.copyTo(_aid_src_gravity.innovation_variance); - float innovation_gate = 1.f; + float innovation_gate = 0.25f; setEstimatorAidStatusTestRatio(_aid_src_gravity, innovation_gate); - const bool accel_clipping = imu.delta_vel_clipping[0] || imu.delta_vel_clipping[1] || imu.delta_vel_clipping[2]; + bool fused[3] {false, false, false}; + + // update the states and covariance using sequential fusion + for (uint8_t index = 0; index <= 2; index++) { + // Calculate Kalman gains and observation jacobians + if (index == 0) { + // everything was already computed above + + } else if (index == 1) { + // recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes) + sym::ComputeGravityYInnovVarAndH(state_vector, P, measurement_var, &_aid_src_gravity.innovation_variance[index], &H); - 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)) - && measurementUpdate(Kz, innovation_variance(2), innovation(2)); + // recalculate innovation using the updated state + _aid_src_gravity.innovation[index] = _state.quat_nominal.rotateVectorInverse(Vector3f(0.f, 0.f, -1.f))(index) - measurement(index); - if (_aid_src_gravity.fused) { - _aid_src_gravity.time_last_fuse = imu.time_us; + } else if (index == 2) { + // recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes) + sym::ComputeGravityZInnovVarAndH(state_vector, P, measurement_var, &_aid_src_gravity.innovation_variance[index], &H); + + // recalculate innovation using the updated state + _aid_src_gravity.innovation[index] = _state.quat_nominal.rotateVectorInverse(Vector3f(0.f, 0.f, -1.f))(index) - measurement(index); } + + VectorState K = P * H / _aid_src_gravity.innovation_variance[index]; + + const bool accel_clipping = imu.delta_vel_clipping[0] || imu.delta_vel_clipping[1] || imu.delta_vel_clipping[2]; + + if (_control_status.flags.gravity_vector && !_aid_src_gravity.innovation_rejected && !accel_clipping) { + fused[index] = measurementUpdate(K, H, _aid_src_gravity.observation_variance[index], _aid_src_gravity.innovation[index]); + } + } + + if (fused[0] && fused[1] && fused[2]) { + _aid_src_gravity.fused = true; + _aid_src_gravity.time_last_fuse = imu.time_us; } } diff --git a/src/modules/ekf2/EKF/mag_3d_control.cpp b/src/modules/ekf2/EKF/mag_3d_control.cpp index 738441cf2704..89f502ac93f8 100644 --- a/src/modules/ekf2/EKF/mag_3d_control.cpp +++ b/src/modules/ekf2/EKF/mag_3d_control.cpp @@ -45,13 +45,12 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star resetEstimatorAidStatus(aid_src); - const bool wmm_updated = (_wmm_gps_time_last_set > aid_src.time_last_fuse); + const bool wmm_updated = (_wmm_gps_time_last_set >= aid_src.time_last_fuse); // WMM update can occur on the last epoch, just after mag fusion // determine if we should use mag fusion bool continuing_conditions_passing = (_params.mag_fusion_type != MagFuseType::NONE) && _control_status.flags.tilt_align && (_control_status.flags.yaw_align || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align)) - && (wmm_updated || checkHaglYawResetReq() || isRecent(_time_last_mov_3d_mag_suitable, (uint64_t)3e6)) && mag_sample.mag.longerThan(0.f) && mag_sample.mag.isAllFinite(); @@ -65,8 +64,6 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star && _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; @@ -92,7 +89,7 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star if (continuing_conditions_passing && _control_status.flags.yaw_align) { - if (mag_sample.reset || checkHaglYawResetReq()) { + if (mag_sample.reset || checkHaglYawResetReq() || wmm_updated) { ECL_INFO("reset to %s", AID_SRC_NAME); resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D); aid_src.time_last_fuse = _time_delayed_us; @@ -159,15 +156,13 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star _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)) + || (getStateVariance().min() < kMagVarianceMin) + || (getStateVariance().min() < kMagVarianceMin) ) { ECL_INFO("starting %s fusion, resetting states", AID_SRC_NAME); @@ -209,25 +204,5 @@ void Ekf::stopMagFusion() _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 cd714e91ecc8..b491228c3999 100644 --- a/src/modules/ekf2/EKF/mag_control.cpp +++ b/src/modules/ekf2/EKF/mag_control.cpp @@ -47,15 +47,9 @@ void Ekf::controlMagFusion() _control_status.flags.mag_aligned_in_flight = false; } - // check mag state observability checkYawAngleObservability(); - checkMagBiasObservability(); checkMagHeadingConsistency(); - if (_mag_bias_observable || _yaw_angle_observable) { - _time_last_mov_3d_mag_suitable = _time_delayed_us; - } - if (_params.mag_fusion_type == MagFuseType::NONE) { stopMagFusion(); stopMagHdgFusion(); @@ -245,24 +239,6 @@ void Ekf::checkYawAngleObservability() } } -void Ekf::checkMagBiasObservability() -{ - // check if there is enough yaw rotation to make the mag bias states observable - if (!_mag_bias_observable && (fabsf(_yaw_rate_lpf_ef) > _params.mag_yaw_rate_gate)) { - // initial yaw motion is detected - _mag_bias_observable = true; - - } else if (_mag_bias_observable) { - // require sustained yaw motion of 50% the initial yaw rate threshold - const float yaw_dt = 1e-6f * (float)(_time_delayed_us - _time_yaw_started); - const float min_yaw_change_req = 0.5f * _params.mag_yaw_rate_gate * yaw_dt; - _mag_bias_observable = fabsf(_yaw_delta_ef) > min_yaw_change_req; - } - - _yaw_delta_ef = 0.0f; - _time_yaw_started = _time_delayed_us; -} - void Ekf::checkMagHeadingConsistency() { if (fabsf(_mag_heading_innov_lpf.getState()) < _params.mag_heading_noise) { diff --git a/src/modules/ekf2/EKF/mag_fusion.cpp b/src/modules/ekf2/EKF/mag_fusion.cpp index 1dc2f31c3bf8..b1a8cc68fe29 100644 --- a/src/modules/ekf2/EKF/mag_fusion.cpp +++ b/src/modules/ekf2/EKF/mag_fusion.cpp @@ -191,7 +191,7 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo Kfusion.slice(State::mag_B.idx, 0) = K_mag_B; } - if (measurementUpdate(Kfusion, aid_src_mag.innovation_variance[index], aid_src_mag.innovation[index])) { + if (measurementUpdate(Kfusion, H, aid_src_mag.observation_variance[index], aid_src_mag.innovation[index])) { fused[index] = true; limitDeclination(); @@ -227,35 +227,52 @@ bool Ekf::fuseDeclination(float decl_sigma) return false; } - // observation variance (rad**2) - const float R_DECL = sq(decl_sigma); + float decl_measurement = NAN; - VectorState H; - float decl_pred; - float innovation_variance; + if ((_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL) + && PX4_ISFINITE(_mag_declination_gps) + ) { + decl_measurement = _mag_declination_gps; - // TODO: review getMagDeclination() usage, use mag_I, _mag_declination_gps, or parameter? - sym::ComputeMagDeclinationPredInnovVarAndH(_state.vector(), P, R_DECL, FLT_EPSILON, &decl_pred, &innovation_variance, &H); + } else if ((_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL) + && PX4_ISFINITE(_params.mag_declination_deg) && (fabsf(_params.mag_declination_deg) > 0.f) + ) { + decl_measurement = math::radians(_params.mag_declination_deg); + } - const float innovation = wrap_pi(decl_pred - getMagDeclination()); + if (PX4_ISFINITE(decl_measurement)) { - if (innovation_variance < R_DECL) { - // variance calculation is badly conditioned - return false; - } + // observation variance (rad**2) + const float R_DECL = sq(decl_sigma); + + VectorState H; + float decl_pred; + float innovation_variance; + + sym::ComputeMagDeclinationPredInnovVarAndH(_state.vector(), P, R_DECL, FLT_EPSILON, &decl_pred, &innovation_variance, &H); - // Calculate the Kalman gains - VectorState Kfusion = P * H / innovation_variance; + const float innovation = wrap_pi(decl_pred - decl_measurement); - const bool is_fused = measurementUpdate(Kfusion, innovation_variance, innovation); + if (innovation_variance < R_DECL) { + // variance calculation is badly conditioned + return false; + } + + // Calculate the Kalman gains + VectorState Kfusion = P * H / innovation_variance; - _fault_status.flags.bad_mag_decl = !is_fused; + const bool is_fused = measurementUpdate(Kfusion, H, R_DECL, innovation); - if (is_fused) { - limitDeclination(); + _fault_status.flags.bad_mag_decl = !is_fused; + + if (is_fused) { + limitDeclination(); + } + + return is_fused; } - return is_fused; + return false; } void Ekf::limitDeclination() @@ -274,7 +291,9 @@ void Ekf::limitDeclination() // set to 50% of the horizontal strength from geo tables if location is known h_field_min = fmaxf(h_field_min, 0.5f * _mag_strength_gps * cosf(_mag_inclination_gps)); - } else if (_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL) { + } else if ((_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL) + && PX4_ISFINITE(_params.mag_declination_deg) && (fabsf(_params.mag_declination_deg) > 0.f) + ) { // use parameter value if GPS isn't available decl_reference = math::radians(_params.mag_declination_deg); } diff --git a/src/modules/ekf2/EKF/optflow_fusion.cpp b/src/modules/ekf2/EKF/optflow_fusion.cpp index 626f0917cd4d..5dd257c7251d 100644 --- a/src/modules/ekf2/EKF/optflow_fusion.cpp +++ b/src/modules/ekf2/EKF/optflow_fusion.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file vel_pos_fusion.cpp + * @file optflow_fusion.cpp * Function for fusing gps and baro measurements/ * equations generated using EKF/python/ekf_derivation/main.py * @@ -146,7 +146,7 @@ void Ekf::fuseOptFlow() 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])) { + if (measurementUpdate(Kfusion, H, _aid_src_optical_flow.observation_variance[index], _aid_src_optical_flow.innovation[index])) { fused[index] = true; } } diff --git a/src/modules/ekf2/EKF/output_predictor.cpp b/src/modules/ekf2/EKF/output_predictor.cpp index 32bf4fe8ad47..93086dc7e6b6 100644 --- a/src/modules/ekf2/EKF/output_predictor.cpp +++ b/src/modules/ekf2/EKF/output_predictor.cpp @@ -41,16 +41,32 @@ using matrix::Vector3f; void OutputPredictor::print_status() { - printf("output predictor: IMU dt: %.4f, EKF dt: %.4f\n", (double)_dt_update_states_avg, (double)_dt_correct_states_avg); + printf("[output predictor] IMU dt: %.6f, EKF dt: %.6f\n", + (double)_dt_update_states_avg, (double)_dt_correct_states_avg); - printf("output predictor: tracking error, angular: %.6f rad, velocity: %.3f m/s, position: %.3f m\n", + const matrix::Quatf q_att = _output_buffer.get_newest().quat_nominal; + const matrix::Eulerf euler = q_att; + + printf("[output predictor] orientation: [%.4f, %.4f, %.4f, %.4f] (Euler [%.3f, %.3f, %.3f])\n", + (double)q_att(0), (double)q_att(1), (double)q_att(2), (double)q_att(3), + (double)euler.phi(), (double)euler.theta(), (double)euler.psi()); + + printf("[output predictor] velocity: [%.3f, %.3f, %.3f]\n", + (double)_output_buffer.get_newest().vel(0), (double)_output_buffer.get_newest().vel(1), + (double)_output_buffer.get_newest().vel(2)); + + printf("[output predictor] position: [%.3f, %.3f, %.3f]\n", + (double)_output_buffer.get_newest().pos(0), (double)_output_buffer.get_newest().pos(1), + (double)_output_buffer.get_newest().pos(2)); + + printf("[output predictor] tracking error, angular: %.6f rad, velocity: %.4f m/s, position: %.4f m\n", (double)_output_tracking_error(0), (double)_output_tracking_error(1), (double)_output_tracking_error(2)); - printf("output buffer: %d/%d (%d Bytes)\n", _output_buffer.entries(), _output_buffer.get_length(), - _output_buffer.get_total_size()); + printf("[output predictor] output buffer: %d/%d (%d Bytes)\n", + _output_buffer.entries(), _output_buffer.get_length(), _output_buffer.get_total_size()); - printf("output vert buffer: %d/%d (%d Bytes)\n", _output_vert_buffer.entries(), _output_vert_buffer.get_length(), - _output_vert_buffer.get_total_size()); + printf("[output predictor] output vert buffer: %d/%d (%d Bytes)\n", + _output_vert_buffer.entries(), _output_vert_buffer.get_length(), _output_vert_buffer.get_total_size()); } void OutputPredictor::alignOutputFilter(const Quatf &quat_state, const Vector3f &vel_state, const Vector3f &pos_state) diff --git a/src/modules/ekf2/EKF/position_fusion.cpp b/src/modules/ekf2/EKF/position_fusion.cpp new file mode 100644 index 000000000000..a1d2f85c273d --- /dev/null +++ b/src/modules/ekf2/EKF/position_fusion.cpp @@ -0,0 +1,210 @@ +/**************************************************************************** + * + * Copyright (c) 2015-2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "ekf.h" + +void Ekf::updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, + const float innov_gate, estimator_aid_source2d_s &aid_src) const +{ + resetEstimatorAidStatus(aid_src); + + for (int i = 0; i < 2; i++) { + aid_src.observation[i] = obs(i); + aid_src.innovation[i] = _state.pos(i) - aid_src.observation[i]; + + aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i)); + const int state_index = State::pos.idx + i; + aid_src.innovation_variance[i] = P(state_index, state_index) + aid_src.observation_variance[i]; + } + + setEstimatorAidStatusTestRatio(aid_src, innov_gate); + + aid_src.timestamp_sample = time_us; +} + +void Ekf::updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const float obs, const float obs_var, + const float innov_gate, estimator_aid_source1d_s &aid_src) const +{ + resetEstimatorAidStatus(aid_src); + + aid_src.observation = obs; + aid_src.innovation = _state.pos(2) - aid_src.observation; + + aid_src.observation_variance = math::max(sq(0.01f), obs_var); + aid_src.innovation_variance = P(State::pos.idx + 2, State::pos.idx + 2) + aid_src.observation_variance; + + setEstimatorAidStatusTestRatio(aid_src, innov_gate); + + // z special case if there is bad vertical acceleration data, then don't reject measurement, + // but limit innovation to prevent spikes that could destabilise the filter + if (_fault_status.flags.bad_acc_vertical && aid_src.innovation_rejected) { + const float innov_limit = innov_gate * sqrtf(aid_src.innovation_variance); + aid_src.innovation = math::constrain(aid_src.innovation, -innov_limit, innov_limit); + aid_src.innovation_rejected = false; + } + + aid_src.timestamp_sample = time_us; +} + +void Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src) +{ + // x & y + if (!aid_src.innovation_rejected + && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0], State::pos.idx + 0) + && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1], State::pos.idx + 1) + ) { + aid_src.fused = true; + aid_src.time_last_fuse = _time_delayed_us; + + _time_last_hor_pos_fuse = _time_delayed_us; + + } else { + aid_src.fused = false; + } +} + +void Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src) +{ + // z + if (!aid_src.innovation_rejected + && fuseDirectStateMeasurement(aid_src.innovation, aid_src.innovation_variance, aid_src.observation_variance, State::pos.idx + 2) + ) { + aid_src.fused = true; + aid_src.time_last_fuse = _time_delayed_us; + + _time_last_hgt_fuse = _time_delayed_us; + + } else { + aid_src.fused = false; + } +} + +void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f &new_horz_pos_var) +{ + const Vector2f delta_horz_pos{new_horz_pos - Vector2f{_state.pos}}; + _state.pos.xy() = new_horz_pos; + + if (PX4_ISFINITE(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>(State::pos.idx + 1, math::max(sq(0.01f), new_horz_pos_var(1))); + } + + _output_predictor.resetHorizontalPositionTo(delta_horz_pos); + + // record the state change + if (_state_reset_status.reset_count.posNE == _state_reset_count_prev.posNE) { + _state_reset_status.posNE_change = delta_horz_pos; + + } else { + // there's already a reset this update, accumulate total delta + _state_reset_status.posNE_change += delta_horz_pos; + } + + _state_reset_status.reset_count.posNE++; + +#if defined(CONFIG_EKF2_EXTERNAL_VISION) + _ev_pos_b_est.setBias(_ev_pos_b_est.getBias() - _state_reset_status.posNE_change); +#endif // CONFIG_EKF2_EXTERNAL_VISION + //_gps_pos_b_est.setBias(_gps_pos_b_est.getBias() + _state_reset_status.posNE_change); + + // Reset the timout timer + _time_last_hor_pos_fuse = _time_delayed_us; +} + +void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_var) +{ + const float old_vert_pos = _state.pos(2); + _state.pos(2) = new_vert_pos; + + if (PX4_ISFINITE(new_vert_pos_var)) { + // the state variance is the same as the observation + 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; + + // apply the change in height / height rate to our newest height / height rate estimate + // which have already been taken out from the output buffer + _output_predictor.resetVerticalPositionTo(new_vert_pos, delta_z); + + // record the state change + if (_state_reset_status.reset_count.posD == _state_reset_count_prev.posD) { + _state_reset_status.posD_change = delta_z; + + } else { + // there's already a reset this update, accumulate total delta + _state_reset_status.posD_change += delta_z; + } + + _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 +#if defined(CONFIG_EKF2_GNSS) + _gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + delta_z); +#endif // CONFIG_EKF2_GNSS +#if defined(CONFIG_EKF2_RANGE_FINDER) + _rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + delta_z); +#endif // CONFIG_EKF2_RANGE_FINDER + +#if defined(CONFIG_EKF2_TERRAIN) + terrainHandleVerticalPositionReset(delta_z); +#endif + + // Reset the timout timer + _time_last_hgt_fuse = _time_delayed_us; +} + +void Ekf::resetHorizontalPositionToLastKnown() +{ + ECL_INFO("reset position to last known (%.3f, %.3f)", (double)_last_known_pos(0), (double)_last_known_pos(1)); + _information_events.flags.reset_pos_to_last_known = true; + + // Used when falling back to non-aiding mode of operation + resetHorizontalPositionTo(_last_known_pos.xy(), sq(_params.pos_noaid_noise)); +} + +void Ekf::resetHorizontalPositionToExternal(const Vector2f &new_horiz_pos, float horiz_accuracy) +{ + ECL_INFO("reset position to external observation"); + _information_events.flags.reset_pos_to_ext_obs = true; + + resetHorizontalPositionTo(new_horiz_pos, sq(horiz_accuracy)); +} diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index 641e7f290d6a..5cbe1a127f3b 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -390,70 +390,20 @@ def compute_mag_z_innov_var_and_h( return (innov_var, H.T) -def compute_yaw_321_innov_var_and_h( +def compute_yaw_innov_var_and_h( state: VState, P: MTangent, - R: sf.Scalar, - epsilon: sf.Scalar -) -> (sf.Scalar, VTangent): - - state = vstate_to_state(state) - R_to_earth = state["quat_nominal"].to_rotation_matrix() - # Fix the singularity at pi/2 by inserting epsilon - meas_pred = sf.atan2(R_to_earth[1,0], R_to_earth[0,0], epsilon=epsilon) - - H = sf.V1(meas_pred).jacobian(state) - innov_var = (H * P * H.T + R)[0,0] - - return (innov_var, H.T) - -def compute_yaw_321_innov_var_and_h_alternate( - state: VState, - P: MTangent, - R: sf.Scalar, - epsilon: sf.Scalar -) -> (sf.Scalar, VTangent): - - state = vstate_to_state(state) - R_to_earth = state["quat_nominal"].to_rotation_matrix() - # Alternate form that has a singularity at yaw 0 instead of pi/2 - meas_pred = sf.pi/2 - sf.atan2(R_to_earth[0,0], R_to_earth[1,0], epsilon=epsilon) - - H = sf.V1(meas_pred).jacobian(state) - innov_var = (H * P * H.T + R)[0,0] - - return (innov_var, H.T) - -def compute_yaw_312_innov_var_and_h( - state: VState, - P: MTangent, - R: sf.Scalar, - epsilon: sf.Scalar -) -> (sf.Scalar, VTangent): - - state = vstate_to_state(state) - R_to_earth = state["quat_nominal"].to_rotation_matrix() - # Alternate form to be used when close to pitch +-pi/2 - meas_pred = sf.atan2(-R_to_earth[0,1], R_to_earth[1,1], epsilon=epsilon) - - H = sf.V1(meas_pred).jacobian(state) - innov_var = (H * P * H.T + R)[0,0] - - return (innov_var, H.T) - -def compute_yaw_312_innov_var_and_h_alternate( - state: VState, - P: MTangent, - R: sf.Scalar, - epsilon: sf.Scalar + R: sf.Scalar ) -> (sf.Scalar, VTangent): state = vstate_to_state(state) - R_to_earth = state["quat_nominal"].to_rotation_matrix() - # Alternate form to be used when close to pitch +-pi/2 - meas_pred = sf.pi/2 - sf.atan2(-R_to_earth[1,1], R_to_earth[0,1], epsilon=epsilon) + q = sf.Quaternion.from_storage(state["quat_nominal"].to_storage()) + r = sf.Quaternion.symbolic('r') + delta_q = q * r.conj() # create a quaternion error of the measurement at the origin + delta_meas_pred = 2 * delta_q.z # Use small angle approximation to obtain a simpler jacobian - H = sf.V1(meas_pred).jacobian(state) + H = sf.V1(delta_meas_pred).jacobian(state) + H = H.subs({r.w: q.w, r.x: q.x, r.y: q.y, r.z: q.z}) # assume innovation is small innov_var = (H * P * H.T + R)[0,0] return (innov_var, H.T) @@ -567,7 +517,7 @@ def predict_drag( return bluff_body_drag + momentum_drag -def compute_drag_x_innov_var_and_k( +def compute_drag_x_innov_var_and_h( state: VState, P: MTangent, rho: sf.Scalar, @@ -581,13 +531,10 @@ def compute_drag_x_innov_var_and_k( meas_pred = predict_drag(state, rho, cd, cm, epsilon) Hx = sf.V1(meas_pred[0]).jacobian(state) innov_var = (Hx * P * Hx.T + R)[0,0] - Ktotal = P * Hx.T / sf.Max(innov_var, epsilon) - K = VTangent() - K[tangent_idx["wind_vel"].idx: tangent_idx["wind_vel"].idx + tangent_idx["wind_vel"].dof] = Ktotal[tangent_idx["wind_vel"].idx: tangent_idx["wind_vel"].idx + tangent_idx["wind_vel"].dof] - return (innov_var, K) + return (innov_var, Hx.T) -def compute_drag_y_innov_var_and_k( +def compute_drag_y_innov_var_and_h( state: VState, P: MTangent, rho: sf.Scalar, @@ -601,41 +548,67 @@ def compute_drag_y_innov_var_and_k( meas_pred = predict_drag(state, rho, cd, cm, epsilon) Hy = sf.V1(meas_pred[1]).jacobian(state) innov_var = (Hy * P * Hy.T + R)[0,0] - Ktotal = P * Hy.T / sf.Max(innov_var, epsilon) - K = VTangent() - K[tangent_idx["wind_vel"].idx: tangent_idx["wind_vel"].idx + tangent_idx["wind_vel"].dof] = Ktotal[tangent_idx["wind_vel"].idx: tangent_idx["wind_vel"].idx + tangent_idx["wind_vel"].dof] - - return (innov_var, K) -def compute_gravity_innov_var_and_k_and_h( - state: VState, - P: MTangent, - meas: sf.V3, - R: sf.Scalar, - epsilon: sf.Scalar -) -> (sf.V3, sf.V3, VTangent, VTangent, VTangent): + return (innov_var, Hy.T) - state = vstate_to_state(state) +def predict_gravity_direction(state: State): # get transform from earth to body frame R_to_body = state["quat_nominal"].inverse() # the innovation is the error between measured acceleration # and predicted (body frame), assuming no body acceleration - meas_pred = R_to_body * sf.Matrix([0,0,-9.80665]) - innov = meas_pred - 9.80665 * meas.normalized(epsilon=epsilon) + return R_to_body * sf.Matrix([0,0,-1]) + +def compute_gravity_xyz_innov_var_and_hx( + state: VState, + P: MTangent, + R: sf.Scalar +) -> (sf.V3, VTangent): + + state = vstate_to_state(state) + meas_pred = predict_gravity_direction(state) # initialize outputs innov_var = sf.V3() - K = [None] * 3 + H = [None] * 3 # calculate observation jacobian (H), kalman gain (K), and innovation variance (S) # for each axis for i in range(3): - H = sf.V1(meas_pred[i]).jacobian(state) - innov_var[i] = (H * P * H.T + R)[0,0] - K[i] = P * H.T / innov_var[i] + H[i] = sf.V1(meas_pred[i]).jacobian(state) + innov_var[i] = (H[i] * P * H[i].T + R)[0,0] + + return (innov_var, H[0].T) - return (innov, innov_var, K[0], K[1], K[2]) +def compute_gravity_y_innov_var_and_h( + state: VState, + P: MTangent, + R: sf.Scalar +) -> (sf.V3, VTangent, VTangent, VTangent): + + state = vstate_to_state(state) + meas_pred = predict_gravity_direction(state) + + # calculate observation jacobian (H), kalman gain (K), and innovation variance (S) + H = sf.V1(meas_pred[1]).jacobian(state) + innov_var = (H * P * H.T + R)[0,0] + + return (innov_var, H.T) + +def compute_gravity_z_innov_var_and_h( + state: VState, + P: MTangent, + R: sf.Scalar +) -> (sf.V3, VTangent, VTangent, VTangent): + + state = vstate_to_state(state) + meas_pred = predict_gravity_direction(state) + + # calculate observation jacobian (H), kalman gain (K), and innovation variance (S) + H = sf.V1(meas_pred[2]).jacobian(state) + innov_var = (H * P * H.T + R)[0,0] + + return (innov_var, H.T) print("Derive EKF2 equations...") generate_px4_function(predict_covariance, output_names=None) @@ -649,19 +622,18 @@ def compute_gravity_innov_var_and_k_and_h( if not args.disable_wind: generate_px4_function(compute_airspeed_h_and_k, output_names=["H", "K"]) generate_px4_function(compute_airspeed_innov_and_innov_var, output_names=["innov", "innov_var"]) - 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_drag_x_innov_var_and_h, output_names=["innov_var", "Hx"]) + generate_px4_function(compute_drag_y_innov_var_and_h, output_names=["innov_var", "Hy"]) generate_px4_function(compute_sideslip_h_and_k, output_names=["H", "K"]) generate_px4_function(compute_sideslip_innov_and_innov_var, output_names=["innov", "innov_var"]) generate_px4_function(compute_wind_init_and_cov_from_airspeed, output_names=["wind", "P_wind"]) -generate_px4_function(compute_yaw_312_innov_var_and_h, output_names=["innov_var", "H"]) -generate_px4_function(compute_yaw_312_innov_var_and_h_alternate, output_names=["innov_var", "H"]) -generate_px4_function(compute_yaw_321_innov_var_and_h, output_names=["innov_var", "H"]) -generate_px4_function(compute_yaw_321_innov_var_and_h_alternate, output_names=["innov_var", "H"]) +generate_px4_function(compute_yaw_innov_var_and_h, output_names=["innov_var", "H"]) generate_px4_function(compute_flow_xy_innov_var_and_hx, output_names=["innov_var", "H"]) generate_px4_function(compute_flow_y_innov_var_and_h, output_names=["innov_var", "H"]) generate_px4_function(compute_gnss_yaw_pred_innov_var_and_h, output_names=["meas_pred", "innov_var", "H"]) -generate_px4_function(compute_gravity_innov_var_and_k_and_h, output_names=["innov", "innov_var", "Kx", "Ky", "Kz"]) +generate_px4_function(compute_gravity_xyz_innov_var_and_hx, output_names=["innov_var", "Hx"]) +generate_px4_function(compute_gravity_y_innov_var_and_h, output_names=["innov_var", "Hy"]) +generate_px4_function(compute_gravity_z_innov_var_and_h, output_names=["innov_var", "Hz"]) generate_px4_state(State, tangent_idx) 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_h.h similarity index 66% rename from src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_x_innov_var_and_k.h rename to src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_x_innov_var_and_h.h index 12c10a917f52..4a08ef76f169 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_h.h @@ -13,7 +13,7 @@ namespace sym { /** * This function was autogenerated from a symbolic function. Do not modify by hand. * - * Symbolic function: compute_drag_x_innov_var_and_k + * Symbolic function: compute_drag_x_innov_var_and_h * * Args: * state: Matrix24_1 @@ -26,21 +26,21 @@ namespace sym { * * Outputs: * innov_var: Scalar - * K: Matrix23_1 + * Hx: Matrix23_1 */ template -void ComputeDragXInnovVarAndK(const matrix::Matrix& state, +void ComputeDragXInnovVarAndH(const matrix::Matrix& state, const matrix::Matrix& P, const Scalar rho, const Scalar cd, const Scalar cm, const Scalar R, const Scalar epsilon, Scalar* const innov_var = nullptr, - matrix::Matrix* const K = nullptr) { - // Total ops: 348 + matrix::Matrix* const Hx = nullptr) { + // Total ops: 317 // Input arrays - // Intermediate terms (77) - const Scalar _tmp0 = 2 * state(0, 0); - const Scalar _tmp1 = _tmp0 * state(3, 0); + // Intermediate terms (73) + const Scalar _tmp0 = 2 * state(3, 0); + const Scalar _tmp1 = _tmp0 * state(0, 0); const Scalar _tmp2 = 2 * state(2, 0); const Scalar _tmp3 = _tmp2 * state(1, 0); const Scalar _tmp4 = _tmp1 + _tmp3; @@ -54,14 +54,14 @@ void ComputeDragXInnovVarAndK(const matrix::Matrix& state, const Scalar _tmp12 = -state(23, 0) + state(5, 0); const Scalar _tmp13 = _tmp2 * state(0, 0); const Scalar _tmp14 = -_tmp13; - const Scalar _tmp15 = 2 * state(1, 0) * state(3, 0); + const Scalar _tmp15 = _tmp0 * state(1, 0); const Scalar _tmp16 = _tmp14 + _tmp15; const Scalar _tmp17 = _tmp12 * _tmp4 + _tmp16 * state(6, 0); const Scalar _tmp18 = _tmp10 * _tmp11 + _tmp17; const Scalar _tmp19 = 2 * _tmp18; const Scalar _tmp20 = _tmp19 * _tmp4; - const Scalar _tmp21 = _tmp2 * state(3, 0); - const Scalar _tmp22 = _tmp0 * state(1, 0); + const Scalar _tmp21 = _tmp0 * state(2, 0); + const Scalar _tmp22 = 2 * state(0, 0) * state(1, 0); const Scalar _tmp23 = -_tmp22; const Scalar _tmp24 = _tmp21 + _tmp23; const Scalar _tmp25 = std::pow(state(1, 0), Scalar(2)); @@ -123,46 +123,44 @@ void ComputeDragXInnovVarAndK(const matrix::Matrix& state, const Scalar _tmp71 = -_tmp43 * (_tmp59 + _tmp60 + _tmp61) - _tmp58 - _tmp62; const Scalar _tmp72 = -_tmp16 * _tmp44 - _tmp16 * cm - _tmp43 * (_tmp16 * _tmp19 + _tmp27 * _tmp31 + _tmp36 * _tmp39); - const Scalar _tmp73 = P(21, 21) * _tmp63; - const Scalar _tmp74 = P(22, 22) * _tmp57; - const Scalar _tmp75 = - R + - _tmp46 * (-P(0, 4) * _tmp67 + P(1, 4) * _tmp70 + P(2, 4) * _tmp56 + P(21, 4) * _tmp63 + - P(22, 4) * _tmp57 + P(3, 4) * _tmp71 + P(4, 4) * _tmp46 + P(5, 4) * _tmp72) + - _tmp56 * (-P(0, 2) * _tmp67 + P(1, 2) * _tmp70 + P(2, 2) * _tmp56 + P(21, 2) * _tmp63 + - P(22, 2) * _tmp57 + P(3, 2) * _tmp71 + P(4, 2) * _tmp46 + P(5, 2) * _tmp72) + - _tmp57 * (-P(0, 22) * _tmp67 + P(1, 22) * _tmp70 + P(2, 22) * _tmp56 + P(21, 22) * _tmp63 + - P(3, 22) * _tmp71 + P(4, 22) * _tmp46 + P(5, 22) * _tmp72 + _tmp74) + - _tmp63 * (-P(0, 21) * _tmp67 + P(1, 21) * _tmp70 + P(2, 21) * _tmp56 + P(22, 21) * _tmp57 + - P(3, 21) * _tmp71 + P(4, 21) * _tmp46 + P(5, 21) * _tmp72 + _tmp73) - - _tmp67 * (-P(0, 0) * _tmp67 + P(1, 0) * _tmp70 + P(2, 0) * _tmp56 + P(21, 0) * _tmp63 + - P(22, 0) * _tmp57 + P(3, 0) * _tmp71 + P(4, 0) * _tmp46 + P(5, 0) * _tmp72) + - _tmp70 * (-P(0, 1) * _tmp67 + P(1, 1) * _tmp70 + P(2, 1) * _tmp56 + P(21, 1) * _tmp63 + - P(22, 1) * _tmp57 + P(3, 1) * _tmp71 + P(4, 1) * _tmp46 + P(5, 1) * _tmp72) + - _tmp71 * (-P(0, 3) * _tmp67 + P(1, 3) * _tmp70 + P(2, 3) * _tmp56 + P(21, 3) * _tmp63 + - P(22, 3) * _tmp57 + P(3, 3) * _tmp71 + P(4, 3) * _tmp46 + P(5, 3) * _tmp72) + - _tmp72 * (-P(0, 5) * _tmp67 + P(1, 5) * _tmp70 + P(2, 5) * _tmp56 + P(21, 5) * _tmp63 + - P(22, 5) * _tmp57 + P(3, 5) * _tmp71 + P(4, 5) * _tmp46 + P(5, 5) * _tmp72); - const Scalar _tmp76 = Scalar(1.0) / (math::max(_tmp75, epsilon)); // Output terms (2) if (innov_var != nullptr) { Scalar& _innov_var = (*innov_var); - _innov_var = _tmp75; + _innov_var = + R + + _tmp46 * (-P(0, 4) * _tmp67 + P(1, 4) * _tmp70 + P(2, 4) * _tmp56 + P(21, 4) * _tmp63 + + P(22, 4) * _tmp57 + P(3, 4) * _tmp71 + P(4, 4) * _tmp46 + P(5, 4) * _tmp72) + + _tmp56 * (-P(0, 2) * _tmp67 + P(1, 2) * _tmp70 + P(2, 2) * _tmp56 + P(21, 2) * _tmp63 + + P(22, 2) * _tmp57 + P(3, 2) * _tmp71 + P(4, 2) * _tmp46 + P(5, 2) * _tmp72) + + _tmp57 * (-P(0, 22) * _tmp67 + P(1, 22) * _tmp70 + P(2, 22) * _tmp56 + P(21, 22) * _tmp63 + + P(22, 22) * _tmp57 + P(3, 22) * _tmp71 + P(4, 22) * _tmp46 + P(5, 22) * _tmp72) + + _tmp63 * (-P(0, 21) * _tmp67 + P(1, 21) * _tmp70 + P(2, 21) * _tmp56 + P(21, 21) * _tmp63 + + P(22, 21) * _tmp57 + P(3, 21) * _tmp71 + P(4, 21) * _tmp46 + P(5, 21) * _tmp72) - + _tmp67 * (-P(0, 0) * _tmp67 + P(1, 0) * _tmp70 + P(2, 0) * _tmp56 + P(21, 0) * _tmp63 + + P(22, 0) * _tmp57 + P(3, 0) * _tmp71 + P(4, 0) * _tmp46 + P(5, 0) * _tmp72) + + _tmp70 * (-P(0, 1) * _tmp67 + P(1, 1) * _tmp70 + P(2, 1) * _tmp56 + P(21, 1) * _tmp63 + + P(22, 1) * _tmp57 + P(3, 1) * _tmp71 + P(4, 1) * _tmp46 + P(5, 1) * _tmp72) + + _tmp71 * (-P(0, 3) * _tmp67 + P(1, 3) * _tmp70 + P(2, 3) * _tmp56 + P(21, 3) * _tmp63 + + P(22, 3) * _tmp57 + P(3, 3) * _tmp71 + P(4, 3) * _tmp46 + P(5, 3) * _tmp72) + + _tmp72 * (-P(0, 5) * _tmp67 + P(1, 5) * _tmp70 + P(2, 5) * _tmp56 + P(21, 5) * _tmp63 + + P(22, 5) * _tmp57 + P(3, 5) * _tmp71 + P(4, 5) * _tmp46 + P(5, 5) * _tmp72); } - if (K != nullptr) { - matrix::Matrix& _k = (*K); + if (Hx != nullptr) { + matrix::Matrix& _hx = (*Hx); - _k.setZero(); + _hx.setZero(); - _k(21, 0) = - _tmp76 * (-P(21, 0) * _tmp67 + P(21, 1) * _tmp70 + P(21, 2) * _tmp56 + P(21, 22) * _tmp57 + - P(21, 3) * _tmp71 + P(21, 4) * _tmp46 + P(21, 5) * _tmp72 + _tmp73); - _k(22, 0) = - _tmp76 * (-P(22, 0) * _tmp67 + P(22, 1) * _tmp70 + P(22, 2) * _tmp56 + P(22, 21) * _tmp63 + - P(22, 3) * _tmp71 + P(22, 4) * _tmp46 + P(22, 5) * _tmp72 + _tmp74); + _hx(0, 0) = -_tmp67; + _hx(1, 0) = _tmp70; + _hx(2, 0) = _tmp56; + _hx(3, 0) = _tmp71; + _hx(4, 0) = _tmp46; + _hx(5, 0) = _tmp72; + _hx(21, 0) = _tmp63; + _hx(22, 0) = _tmp57; } } // NOLINT(readability/fn_size) 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_h.h similarity index 69% rename from src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_y_innov_var_and_k.h rename to src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_y_innov_var_and_h.h index 43a129cd2606..c504d0db636a 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_h.h @@ -13,7 +13,7 @@ namespace sym { /** * This function was autogenerated from a symbolic function. Do not modify by hand. * - * Symbolic function: compute_drag_y_innov_var_and_k + * Symbolic function: compute_drag_y_innov_var_and_h * * Args: * state: Matrix24_1 @@ -26,19 +26,19 @@ namespace sym { * * Outputs: * innov_var: Scalar - * K: Matrix23_1 + * Hy: Matrix23_1 */ template -void ComputeDragYInnovVarAndK(const matrix::Matrix& state, +void ComputeDragYInnovVarAndH(const matrix::Matrix& state, const matrix::Matrix& P, const Scalar rho, const Scalar cd, const Scalar cm, const Scalar R, const Scalar epsilon, Scalar* const innov_var = nullptr, - matrix::Matrix* const K = nullptr) { - // Total ops: 348 + matrix::Matrix* const Hy = nullptr) { + // Total ops: 317 // Input arrays - // Intermediate terms (77) + // Intermediate terms (73) const Scalar _tmp0 = 2 * state(2, 0) * state(3, 0); const Scalar _tmp1 = 2 * state(1, 0); const Scalar _tmp2 = _tmp1 * state(0, 0); @@ -125,46 +125,44 @@ void ComputeDragYInnovVarAndK(const matrix::Matrix& state, const Scalar _tmp70 = _tmp31 * _tmp39; const Scalar _tmp71 = -_tmp40 * (_tmp68 + _tmp69 + _tmp70) - _tmp66 - _tmp67; const Scalar _tmp72 = -_tmp40 * (-_tmp68 - _tmp69 - _tmp70) + _tmp66 + _tmp67; - const Scalar _tmp73 = P(22, 22) * _tmp62; - const Scalar _tmp74 = P(21, 21) * _tmp72; - const Scalar _tmp75 = - R + - _tmp41 * (P(0, 5) * _tmp51 - P(1, 5) * _tmp64 + P(2, 5) * _tmp56 + P(21, 5) * _tmp72 + - P(22, 5) * _tmp62 + P(3, 5) * _tmp71 + P(4, 5) * _tmp65 + P(5, 5) * _tmp41) + - _tmp51 * (P(0, 0) * _tmp51 - P(1, 0) * _tmp64 + P(2, 0) * _tmp56 + P(21, 0) * _tmp72 + - P(22, 0) * _tmp62 + P(3, 0) * _tmp71 + P(4, 0) * _tmp65 + P(5, 0) * _tmp41) + - _tmp56 * (P(0, 2) * _tmp51 - P(1, 2) * _tmp64 + P(2, 2) * _tmp56 + P(21, 2) * _tmp72 + - P(22, 2) * _tmp62 + P(3, 2) * _tmp71 + P(4, 2) * _tmp65 + P(5, 2) * _tmp41) + - _tmp62 * (P(0, 22) * _tmp51 - P(1, 22) * _tmp64 + P(2, 22) * _tmp56 + P(21, 22) * _tmp72 + - P(3, 22) * _tmp71 + P(4, 22) * _tmp65 + P(5, 22) * _tmp41 + _tmp73) - - _tmp64 * (P(0, 1) * _tmp51 - P(1, 1) * _tmp64 + P(2, 1) * _tmp56 + P(21, 1) * _tmp72 + - P(22, 1) * _tmp62 + P(3, 1) * _tmp71 + P(4, 1) * _tmp65 + P(5, 1) * _tmp41) + - _tmp65 * (P(0, 4) * _tmp51 - P(1, 4) * _tmp64 + P(2, 4) * _tmp56 + P(21, 4) * _tmp72 + - P(22, 4) * _tmp62 + P(3, 4) * _tmp71 + P(4, 4) * _tmp65 + P(5, 4) * _tmp41) + - _tmp71 * (P(0, 3) * _tmp51 - P(1, 3) * _tmp64 + P(2, 3) * _tmp56 + P(21, 3) * _tmp72 + - P(22, 3) * _tmp62 + P(3, 3) * _tmp71 + P(4, 3) * _tmp65 + P(5, 3) * _tmp41) + - _tmp72 * (P(0, 21) * _tmp51 - P(1, 21) * _tmp64 + P(2, 21) * _tmp56 + P(22, 21) * _tmp62 + - P(3, 21) * _tmp71 + P(4, 21) * _tmp65 + P(5, 21) * _tmp41 + _tmp74); - const Scalar _tmp76 = Scalar(1.0) / (math::max(_tmp75, epsilon)); // Output terms (2) if (innov_var != nullptr) { Scalar& _innov_var = (*innov_var); - _innov_var = _tmp75; + _innov_var = + R + + _tmp41 * (P(0, 5) * _tmp51 - P(1, 5) * _tmp64 + P(2, 5) * _tmp56 + P(21, 5) * _tmp72 + + P(22, 5) * _tmp62 + P(3, 5) * _tmp71 + P(4, 5) * _tmp65 + P(5, 5) * _tmp41) + + _tmp51 * (P(0, 0) * _tmp51 - P(1, 0) * _tmp64 + P(2, 0) * _tmp56 + P(21, 0) * _tmp72 + + P(22, 0) * _tmp62 + P(3, 0) * _tmp71 + P(4, 0) * _tmp65 + P(5, 0) * _tmp41) + + _tmp56 * (P(0, 2) * _tmp51 - P(1, 2) * _tmp64 + P(2, 2) * _tmp56 + P(21, 2) * _tmp72 + + P(22, 2) * _tmp62 + P(3, 2) * _tmp71 + P(4, 2) * _tmp65 + P(5, 2) * _tmp41) + + _tmp62 * (P(0, 22) * _tmp51 - P(1, 22) * _tmp64 + P(2, 22) * _tmp56 + P(21, 22) * _tmp72 + + P(22, 22) * _tmp62 + P(3, 22) * _tmp71 + P(4, 22) * _tmp65 + P(5, 22) * _tmp41) - + _tmp64 * (P(0, 1) * _tmp51 - P(1, 1) * _tmp64 + P(2, 1) * _tmp56 + P(21, 1) * _tmp72 + + P(22, 1) * _tmp62 + P(3, 1) * _tmp71 + P(4, 1) * _tmp65 + P(5, 1) * _tmp41) + + _tmp65 * (P(0, 4) * _tmp51 - P(1, 4) * _tmp64 + P(2, 4) * _tmp56 + P(21, 4) * _tmp72 + + P(22, 4) * _tmp62 + P(3, 4) * _tmp71 + P(4, 4) * _tmp65 + P(5, 4) * _tmp41) + + _tmp71 * (P(0, 3) * _tmp51 - P(1, 3) * _tmp64 + P(2, 3) * _tmp56 + P(21, 3) * _tmp72 + + P(22, 3) * _tmp62 + P(3, 3) * _tmp71 + P(4, 3) * _tmp65 + P(5, 3) * _tmp41) + + _tmp72 * (P(0, 21) * _tmp51 - P(1, 21) * _tmp64 + P(2, 21) * _tmp56 + P(21, 21) * _tmp72 + + P(22, 21) * _tmp62 + P(3, 21) * _tmp71 + P(4, 21) * _tmp65 + P(5, 21) * _tmp41); } - if (K != nullptr) { - matrix::Matrix& _k = (*K); + if (Hy != nullptr) { + matrix::Matrix& _hy = (*Hy); - _k.setZero(); + _hy.setZero(); - _k(21, 0) = - _tmp76 * (P(21, 0) * _tmp51 - P(21, 1) * _tmp64 + P(21, 2) * _tmp56 + P(21, 22) * _tmp62 + - P(21, 3) * _tmp71 + P(21, 4) * _tmp65 + P(21, 5) * _tmp41 + _tmp74); - _k(22, 0) = - _tmp76 * (P(22, 0) * _tmp51 - P(22, 1) * _tmp64 + P(22, 2) * _tmp56 + P(22, 21) * _tmp72 + - P(22, 3) * _tmp71 + P(22, 4) * _tmp65 + P(22, 5) * _tmp41 + _tmp73); + _hy(0, 0) = _tmp51; + _hy(1, 0) = -_tmp64; + _hy(2, 0) = _tmp56; + _hy(3, 0) = _tmp71; + _hy(4, 0) = _tmp65; + _hy(5, 0) = _tmp41; + _hy(21, 0) = _tmp72; + _hy(22, 0) = _tmp62; } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_innov_var_and_k_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_innov_var_and_k_and_h.h deleted file mode 100644 index 2ac6d93aa3e6..000000000000 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_innov_var_and_k_and_h.h +++ /dev/null @@ -1,191 +0,0 @@ -// ----------------------------------------------------------------------------- -// This file was autogenerated by symforce from template: -// function/FUNCTION.h.jinja -// Do NOT modify by hand. -// ----------------------------------------------------------------------------- - -#pragma once - -#include - -namespace sym { - -/** - * This function was autogenerated from a symbolic function. Do not modify by hand. - * - * Symbolic function: compute_gravity_innov_var_and_k_and_h - * - * Args: - * state: Matrix24_1 - * P: Matrix23_23 - * meas: Matrix31 - * R: Scalar - * epsilon: Scalar - * - * Outputs: - * innov: Matrix31 - * innov_var: Matrix31 - * Kx: Matrix23_1 - * Ky: Matrix23_1 - * Kz: Matrix23_1 - */ -template -void ComputeGravityInnovVarAndKAndH(const matrix::Matrix& state, - const matrix::Matrix& P, - const matrix::Matrix& meas, const Scalar R, - const Scalar epsilon, - matrix::Matrix* const innov = nullptr, - matrix::Matrix* const innov_var = nullptr, - matrix::Matrix* const Kx = nullptr, - matrix::Matrix* const Ky = nullptr, - matrix::Matrix* const Kz = nullptr) { - // Total ops: 361 - - // Input arrays - - // Intermediate terms (31) - const Scalar _tmp0 = 2 * state(2, 0); - const Scalar _tmp1 = _tmp0 * state(0, 0); - const Scalar _tmp2 = 2 * state(1, 0); - const Scalar _tmp3 = _tmp2 * state(3, 0); - const Scalar _tmp4 = -_tmp1 + _tmp3; - const Scalar _tmp5 = - Scalar(9.8066499999999994) / - std::sqrt(Scalar(epsilon + std::pow(meas(0, 0), Scalar(2)) + std::pow(meas(1, 0), Scalar(2)) + - std::pow(meas(2, 0), Scalar(2)))); - const Scalar _tmp6 = _tmp0 * state(3, 0); - const Scalar _tmp7 = _tmp2 * state(0, 0); - const Scalar _tmp8 = _tmp6 + _tmp7; - const Scalar _tmp9 = std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp10 = std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp11 = std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp12 = std::pow(state(0, 0), Scalar(2)); - const Scalar _tmp13 = -Scalar(9.8066499999999994) * _tmp10 + Scalar(9.8066499999999994) * _tmp11 + - Scalar(9.8066499999999994) * _tmp12 - Scalar(9.8066499999999994) * _tmp9; - const Scalar _tmp14 = -Scalar(9.8066499999999994) * _tmp8; - const Scalar _tmp15 = P(2, 2) * _tmp14; - const Scalar _tmp16 = P(1, 1) * _tmp13; - const Scalar _tmp17 = - R + _tmp13 * (P(2, 1) * _tmp14 + _tmp16) + _tmp14 * (P(1, 2) * _tmp13 + _tmp15); - const Scalar _tmp18 = Scalar(9.8066499999999994) * _tmp10 - Scalar(9.8066499999999994) * _tmp11 - - Scalar(9.8066499999999994) * _tmp12 + Scalar(9.8066499999999994) * _tmp9; - const Scalar _tmp19 = -Scalar(9.8066499999999994) * _tmp1 + Scalar(9.8066499999999994) * _tmp3; - const Scalar _tmp20 = P(2, 2) * _tmp19; - const Scalar _tmp21 = P(0, 0) * _tmp18; - const Scalar _tmp22 = - R + _tmp18 * (P(2, 0) * _tmp19 + _tmp21) + _tmp19 * (P(0, 2) * _tmp18 + _tmp20); - const Scalar _tmp23 = Scalar(9.8066499999999994) * _tmp6 + Scalar(9.8066499999999994) * _tmp7; - const Scalar _tmp24 = -Scalar(9.8066499999999994) * _tmp4; - const Scalar _tmp25 = P(1, 1) * _tmp24; - const Scalar _tmp26 = P(0, 0) * _tmp23; - const Scalar _tmp27 = - R + _tmp23 * (P(1, 0) * _tmp24 + _tmp26) + _tmp24 * (P(0, 1) * _tmp23 + _tmp25); - const Scalar _tmp28 = Scalar(1.0) / (_tmp17); - const Scalar _tmp29 = Scalar(1.0) / (_tmp22); - const Scalar _tmp30 = Scalar(1.0) / (_tmp27); - - // Output terms (5) - if (innov != nullptr) { - matrix::Matrix& _innov = (*innov); - - _innov(0, 0) = -Scalar(9.8066499999999994) * _tmp4 - _tmp5 * meas(0, 0); - _innov(1, 0) = -_tmp5 * meas(1, 0) - Scalar(9.8066499999999994) * _tmp8; - _innov(2, 0) = Scalar(19.613299999999999) * _tmp10 - _tmp5 * meas(2, 0) + - Scalar(19.613299999999999) * _tmp9 + Scalar(-9.8066499999999994); - } - - if (innov_var != nullptr) { - matrix::Matrix& _innov_var = (*innov_var); - - _innov_var(0, 0) = _tmp17; - _innov_var(1, 0) = _tmp22; - _innov_var(2, 0) = _tmp27; - } - - if (Kx != nullptr) { - matrix::Matrix& _kx = (*Kx); - - _kx(0, 0) = _tmp28 * (P(0, 1) * _tmp13 + P(0, 2) * _tmp14); - _kx(1, 0) = _tmp28 * (P(1, 2) * _tmp14 + _tmp16); - _kx(2, 0) = _tmp28 * (P(2, 1) * _tmp13 + _tmp15); - _kx(3, 0) = _tmp28 * (P(3, 1) * _tmp13 + P(3, 2) * _tmp14); - _kx(4, 0) = _tmp28 * (P(4, 1) * _tmp13 + P(4, 2) * _tmp14); - _kx(5, 0) = _tmp28 * (P(5, 1) * _tmp13 + P(5, 2) * _tmp14); - _kx(6, 0) = _tmp28 * (P(6, 1) * _tmp13 + P(6, 2) * _tmp14); - _kx(7, 0) = _tmp28 * (P(7, 1) * _tmp13 + P(7, 2) * _tmp14); - _kx(8, 0) = _tmp28 * (P(8, 1) * _tmp13 + P(8, 2) * _tmp14); - _kx(9, 0) = _tmp28 * (P(9, 1) * _tmp13 + P(9, 2) * _tmp14); - _kx(10, 0) = _tmp28 * (P(10, 1) * _tmp13 + P(10, 2) * _tmp14); - _kx(11, 0) = _tmp28 * (P(11, 1) * _tmp13 + P(11, 2) * _tmp14); - _kx(12, 0) = _tmp28 * (P(12, 1) * _tmp13 + P(12, 2) * _tmp14); - _kx(13, 0) = _tmp28 * (P(13, 1) * _tmp13 + P(13, 2) * _tmp14); - _kx(14, 0) = _tmp28 * (P(14, 1) * _tmp13 + P(14, 2) * _tmp14); - _kx(15, 0) = _tmp28 * (P(15, 1) * _tmp13 + P(15, 2) * _tmp14); - _kx(16, 0) = _tmp28 * (P(16, 1) * _tmp13 + P(16, 2) * _tmp14); - _kx(17, 0) = _tmp28 * (P(17, 1) * _tmp13 + P(17, 2) * _tmp14); - _kx(18, 0) = _tmp28 * (P(18, 1) * _tmp13 + P(18, 2) * _tmp14); - _kx(19, 0) = _tmp28 * (P(19, 1) * _tmp13 + P(19, 2) * _tmp14); - _kx(20, 0) = _tmp28 * (P(20, 1) * _tmp13 + P(20, 2) * _tmp14); - _kx(21, 0) = _tmp28 * (P(21, 1) * _tmp13 + P(21, 2) * _tmp14); - _kx(22, 0) = _tmp28 * (P(22, 1) * _tmp13 + P(22, 2) * _tmp14); - } - - if (Ky != nullptr) { - matrix::Matrix& _ky = (*Ky); - - _ky(0, 0) = _tmp29 * (P(0, 2) * _tmp19 + _tmp21); - _ky(1, 0) = _tmp29 * (P(1, 0) * _tmp18 + P(1, 2) * _tmp19); - _ky(2, 0) = _tmp29 * (P(2, 0) * _tmp18 + _tmp20); - _ky(3, 0) = _tmp29 * (P(3, 0) * _tmp18 + P(3, 2) * _tmp19); - _ky(4, 0) = _tmp29 * (P(4, 0) * _tmp18 + P(4, 2) * _tmp19); - _ky(5, 0) = _tmp29 * (P(5, 0) * _tmp18 + P(5, 2) * _tmp19); - _ky(6, 0) = _tmp29 * (P(6, 0) * _tmp18 + P(6, 2) * _tmp19); - _ky(7, 0) = _tmp29 * (P(7, 0) * _tmp18 + P(7, 2) * _tmp19); - _ky(8, 0) = _tmp29 * (P(8, 0) * _tmp18 + P(8, 2) * _tmp19); - _ky(9, 0) = _tmp29 * (P(9, 0) * _tmp18 + P(9, 2) * _tmp19); - _ky(10, 0) = _tmp29 * (P(10, 0) * _tmp18 + P(10, 2) * _tmp19); - _ky(11, 0) = _tmp29 * (P(11, 0) * _tmp18 + P(11, 2) * _tmp19); - _ky(12, 0) = _tmp29 * (P(12, 0) * _tmp18 + P(12, 2) * _tmp19); - _ky(13, 0) = _tmp29 * (P(13, 0) * _tmp18 + P(13, 2) * _tmp19); - _ky(14, 0) = _tmp29 * (P(14, 0) * _tmp18 + P(14, 2) * _tmp19); - _ky(15, 0) = _tmp29 * (P(15, 0) * _tmp18 + P(15, 2) * _tmp19); - _ky(16, 0) = _tmp29 * (P(16, 0) * _tmp18 + P(16, 2) * _tmp19); - _ky(17, 0) = _tmp29 * (P(17, 0) * _tmp18 + P(17, 2) * _tmp19); - _ky(18, 0) = _tmp29 * (P(18, 0) * _tmp18 + P(18, 2) * _tmp19); - _ky(19, 0) = _tmp29 * (P(19, 0) * _tmp18 + P(19, 2) * _tmp19); - _ky(20, 0) = _tmp29 * (P(20, 0) * _tmp18 + P(20, 2) * _tmp19); - _ky(21, 0) = _tmp29 * (P(21, 0) * _tmp18 + P(21, 2) * _tmp19); - _ky(22, 0) = _tmp29 * (P(22, 0) * _tmp18 + P(22, 2) * _tmp19); - } - - if (Kz != nullptr) { - matrix::Matrix& _kz = (*Kz); - - _kz(0, 0) = _tmp30 * (P(0, 1) * _tmp24 + _tmp26); - _kz(1, 0) = _tmp30 * (P(1, 0) * _tmp23 + _tmp25); - _kz(2, 0) = _tmp30 * (P(2, 0) * _tmp23 + P(2, 1) * _tmp24); - _kz(3, 0) = _tmp30 * (P(3, 0) * _tmp23 + P(3, 1) * _tmp24); - _kz(4, 0) = _tmp30 * (P(4, 0) * _tmp23 + P(4, 1) * _tmp24); - _kz(5, 0) = _tmp30 * (P(5, 0) * _tmp23 + P(5, 1) * _tmp24); - _kz(6, 0) = _tmp30 * (P(6, 0) * _tmp23 + P(6, 1) * _tmp24); - _kz(7, 0) = _tmp30 * (P(7, 0) * _tmp23 + P(7, 1) * _tmp24); - _kz(8, 0) = _tmp30 * (P(8, 0) * _tmp23 + P(8, 1) * _tmp24); - _kz(9, 0) = _tmp30 * (P(9, 0) * _tmp23 + P(9, 1) * _tmp24); - _kz(10, 0) = _tmp30 * (P(10, 0) * _tmp23 + P(10, 1) * _tmp24); - _kz(11, 0) = _tmp30 * (P(11, 0) * _tmp23 + P(11, 1) * _tmp24); - _kz(12, 0) = _tmp30 * (P(12, 0) * _tmp23 + P(12, 1) * _tmp24); - _kz(13, 0) = _tmp30 * (P(13, 0) * _tmp23 + P(13, 1) * _tmp24); - _kz(14, 0) = _tmp30 * (P(14, 0) * _tmp23 + P(14, 1) * _tmp24); - _kz(15, 0) = _tmp30 * (P(15, 0) * _tmp23 + P(15, 1) * _tmp24); - _kz(16, 0) = _tmp30 * (P(16, 0) * _tmp23 + P(16, 1) * _tmp24); - _kz(17, 0) = _tmp30 * (P(17, 0) * _tmp23 + P(17, 1) * _tmp24); - _kz(18, 0) = _tmp30 * (P(18, 0) * _tmp23 + P(18, 1) * _tmp24); - _kz(19, 0) = _tmp30 * (P(19, 0) * _tmp23 + P(19, 1) * _tmp24); - _kz(20, 0) = _tmp30 * (P(20, 0) * _tmp23 + P(20, 1) * _tmp24); - _kz(21, 0) = _tmp30 * (P(21, 0) * _tmp23 + P(21, 1) * _tmp24); - _kz(22, 0) = _tmp30 * (P(22, 0) * _tmp23 + P(22, 1) * _tmp24); - } -} // NOLINT(readability/fn_size) - -// NOLINTNEXTLINE(readability/fn_size) -} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_xyz_innov_var_and_hx.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_xyz_innov_var_and_hx.h new file mode 100644 index 000000000000..f3f074e82db7 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_xyz_innov_var_and_hx.h @@ -0,0 +1,77 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +namespace sym { + +/** + * This function was autogenerated from a symbolic function. Do not modify by hand. + * + * Symbolic function: compute_gravity_xyz_innov_var_and_hx + * + * Args: + * state: Matrix24_1 + * P: Matrix23_23 + * R: Scalar + * + * Outputs: + * innov_var: Matrix31 + * Hx: Matrix23_1 + */ +template +void ComputeGravityXyzInnovVarAndHx(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar R, + matrix::Matrix* const innov_var = nullptr, + matrix::Matrix* const Hx = nullptr) { + // Total ops: 51 + + // Input arrays + + // Intermediate terms (16) + const Scalar _tmp0 = std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp1 = std::pow(state(0, 0), Scalar(2)); + const Scalar _tmp2 = std::pow(state(2, 0), Scalar(2)); + const Scalar _tmp3 = std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp4 = _tmp0 + _tmp1 - _tmp2 - _tmp3; + const Scalar _tmp5 = 2 * state(2, 0); + const Scalar _tmp6 = _tmp5 * state(3, 0); + const Scalar _tmp7 = 2 * state(1, 0); + const Scalar _tmp8 = _tmp7 * state(0, 0); + const Scalar _tmp9 = -_tmp6 - _tmp8; + const Scalar _tmp10 = -_tmp0 - _tmp1 + _tmp2 + _tmp3; + const Scalar _tmp11 = _tmp5 * state(0, 0); + const Scalar _tmp12 = _tmp7 * state(3, 0); + const Scalar _tmp13 = -_tmp11 + _tmp12; + const Scalar _tmp14 = _tmp6 + _tmp8; + const Scalar _tmp15 = _tmp11 - _tmp12; + + // Output terms (2) + if (innov_var != nullptr) { + matrix::Matrix& _innov_var = (*innov_var); + + _innov_var(0, 0) = R + _tmp4 * (P(1, 1) * _tmp4 + P(2, 1) * _tmp9) + + _tmp9 * (P(1, 2) * _tmp4 + P(2, 2) * _tmp9); + _innov_var(1, 0) = R + _tmp10 * (P(0, 0) * _tmp10 + P(2, 0) * _tmp13) + + _tmp13 * (P(0, 2) * _tmp10 + P(2, 2) * _tmp13); + _innov_var(2, 0) = R + _tmp14 * (P(0, 0) * _tmp14 + P(1, 0) * _tmp15) + + _tmp15 * (P(0, 1) * _tmp14 + P(1, 1) * _tmp15); + } + + if (Hx != nullptr) { + matrix::Matrix& _hx = (*Hx); + + _hx.setZero(); + + _hx(1, 0) = _tmp4; + _hx(2, 0) = _tmp9; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_y_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_y_innov_var_and_h.h new file mode 100644 index 000000000000..2c3a11b682e5 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_y_innov_var_and_h.h @@ -0,0 +1,60 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +namespace sym { + +/** + * This function was autogenerated from a symbolic function. Do not modify by hand. + * + * Symbolic function: compute_gravity_y_innov_var_and_h + * + * Args: + * state: Matrix24_1 + * P: Matrix23_23 + * R: Scalar + * + * Outputs: + * innov_var: Scalar + * Hy: Matrix23_1 + */ +template +void ComputeGravityYInnovVarAndH(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar R, + Scalar* const innov_var = nullptr, + matrix::Matrix* const Hy = nullptr) { + // Total ops: 22 + + // Input arrays + + // Intermediate terms (2) + const Scalar _tmp0 = -std::pow(state(0, 0), Scalar(2)) + std::pow(state(1, 0), Scalar(2)) + + std::pow(state(2, 0), Scalar(2)) - std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp1 = -2 * state(0, 0) * state(2, 0) + 2 * state(1, 0) * state(3, 0); + + // Output terms (2) + if (innov_var != nullptr) { + Scalar& _innov_var = (*innov_var); + + _innov_var = R + _tmp0 * (P(0, 0) * _tmp0 + P(2, 0) * _tmp1) + + _tmp1 * (P(0, 2) * _tmp0 + P(2, 2) * _tmp1); + } + + if (Hy != nullptr) { + matrix::Matrix& _hy = (*Hy); + + _hy.setZero(); + + _hy(0, 0) = _tmp0; + _hy(2, 0) = _tmp1; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_z_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_z_innov_var_and_h.h new file mode 100644 index 000000000000..43d48e128321 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_z_innov_var_and_h.h @@ -0,0 +1,61 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +namespace sym { + +/** + * This function was autogenerated from a symbolic function. Do not modify by hand. + * + * Symbolic function: compute_gravity_z_innov_var_and_h + * + * Args: + * state: Matrix24_1 + * P: Matrix23_23 + * R: Scalar + * + * Outputs: + * innov_var: Scalar + * Hz: Matrix23_1 + */ +template +void ComputeGravityZInnovVarAndH(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar R, + Scalar* const innov_var = nullptr, + matrix::Matrix* const Hz = nullptr) { + // Total ops: 18 + + // Input arrays + + // Intermediate terms (4) + const Scalar _tmp0 = 2 * state(2, 0); + const Scalar _tmp1 = 2 * state(1, 0); + const Scalar _tmp2 = _tmp0 * state(3, 0) + _tmp1 * state(0, 0); + const Scalar _tmp3 = _tmp0 * state(0, 0) - _tmp1 * state(3, 0); + + // Output terms (2) + if (innov_var != nullptr) { + Scalar& _innov_var = (*innov_var); + + _innov_var = R + _tmp2 * (P(0, 0) * _tmp2 + P(1, 0) * _tmp3) + + _tmp3 * (P(0, 1) * _tmp2 + P(1, 1) * _tmp3); + } + + if (Hz != nullptr) { + matrix::Matrix& _hz = (*Hz); + + _hz.setZero(); + + _hz(0, 0) = _tmp2; + _hz(1, 0) = _tmp3; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym 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 deleted file mode 100644 index e6fd32157ef7..000000000000 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h.h +++ /dev/null @@ -1,76 +0,0 @@ -// ----------------------------------------------------------------------------- -// This file was autogenerated by symforce from template: -// function/FUNCTION.h.jinja -// Do NOT modify by hand. -// ----------------------------------------------------------------------------- - -#pragma once - -#include - -namespace sym { - -/** - * This function was autogenerated from a symbolic function. Do not modify by hand. - * - * Symbolic function: compute_yaw_312_innov_var_and_h - * - * Args: - * state: Matrix24_1 - * P: Matrix23_23 - * R: Scalar - * epsilon: Scalar - * - * Outputs: - * innov_var: Scalar - * H: Matrix23_1 - */ -template -void ComputeYaw312InnovVarAndH(const matrix::Matrix& state, - const matrix::Matrix& P, const Scalar R, - const Scalar epsilon, Scalar* const innov_var = nullptr, - matrix::Matrix* const H = nullptr) { - // Total ops: 53 - - // Input arrays - - // Intermediate terms (15) - const Scalar _tmp0 = 2 * state(0, 0); - const Scalar _tmp1 = -_tmp0 * state(3, 0); - const Scalar _tmp2 = 2 * state(1, 0); - const Scalar _tmp3 = _tmp2 * state(2, 0); - const Scalar _tmp4 = _tmp1 + _tmp3; - const Scalar _tmp5 = std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp6 = std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp7 = -2 * _tmp5 - 2 * _tmp6 + 1; - const Scalar _tmp8 = _tmp7 + epsilon * ((((_tmp7) > 0) - ((_tmp7) < 0)) + Scalar(0.5)); - const Scalar _tmp9 = std::pow(_tmp8, Scalar(2)); - const Scalar _tmp10 = _tmp4 / _tmp9; - const Scalar _tmp11 = Scalar(1.0) / (_tmp8); - const Scalar _tmp12 = _tmp9 / (std::pow(_tmp4, Scalar(2)) + _tmp9); - const Scalar _tmp13 = _tmp12 * (_tmp10 * (_tmp1 - _tmp3) - - _tmp11 * (_tmp5 - _tmp6 - std::pow(state(0, 0), Scalar(2)) + - std::pow(state(2, 0), Scalar(2)))); - const Scalar _tmp14 = _tmp12 * (_tmp10 * (-_tmp2 * state(0, 0) + 2 * state(2, 0) * state(3, 0)) - - _tmp11 * (_tmp0 * state(2, 0) + _tmp2 * state(3, 0))); - - // Output terms (2) - if (innov_var != nullptr) { - Scalar& _innov_var = (*innov_var); - - _innov_var = R + _tmp13 * (P(0, 2) * _tmp14 + P(2, 2) * _tmp13) + - _tmp14 * (P(0, 0) * _tmp14 + P(2, 0) * _tmp13); - } - - if (H != nullptr) { - matrix::Matrix& _h = (*H); - - _h.setZero(); - - _h(0, 0) = _tmp14; - _h(2, 0) = _tmp13; - } -} // NOLINT(readability/fn_size) - -// NOLINTNEXTLINE(readability/fn_size) -} // namespace sym 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 deleted file mode 100644 index 07fd1091819a..000000000000 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h_alternate.h +++ /dev/null @@ -1,76 +0,0 @@ -// ----------------------------------------------------------------------------- -// This file was autogenerated by symforce from template: -// function/FUNCTION.h.jinja -// Do NOT modify by hand. -// ----------------------------------------------------------------------------- - -#pragma once - -#include - -namespace sym { - -/** - * This function was autogenerated from a symbolic function. Do not modify by hand. - * - * Symbolic function: compute_yaw_312_innov_var_and_h_alternate - * - * Args: - * state: Matrix24_1 - * P: Matrix23_23 - * R: Scalar - * epsilon: Scalar - * - * Outputs: - * innov_var: Scalar - * H: Matrix23_1 - */ -template -void ComputeYaw312InnovVarAndHAlternate(const matrix::Matrix& state, - const matrix::Matrix& P, const Scalar R, - const Scalar epsilon, Scalar* const innov_var = nullptr, - matrix::Matrix* const H = nullptr) { - // Total ops: 57 - - // Input arrays - - // Intermediate terms (15) - const Scalar _tmp0 = 2 * state(0, 0); - const Scalar _tmp1 = 2 * state(1, 0); - const Scalar _tmp2 = std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp3 = std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp4 = -2 * _tmp2 - 2 * _tmp3 + 1; - const Scalar _tmp5 = -_tmp0 * state(3, 0); - const Scalar _tmp6 = _tmp1 * state(2, 0); - const Scalar _tmp7 = _tmp5 + _tmp6; - const Scalar _tmp8 = _tmp7 + epsilon * ((((_tmp7) > 0) - ((_tmp7) < 0)) + Scalar(0.5)); - const Scalar _tmp9 = std::pow(_tmp8, Scalar(2)); - const Scalar _tmp10 = _tmp4 / _tmp9; - const Scalar _tmp11 = Scalar(1.0) / (_tmp8); - const Scalar _tmp12 = _tmp9 / (std::pow(_tmp4, Scalar(2)) + _tmp9); - const Scalar _tmp13 = _tmp12 * (_tmp10 * (_tmp0 * state(2, 0) + _tmp1 * state(3, 0)) - - _tmp11 * (-_tmp1 * state(0, 0) + 2 * state(2, 0) * state(3, 0))); - const Scalar _tmp14 = _tmp12 * (_tmp10 * (_tmp2 - _tmp3 - std::pow(state(0, 0), Scalar(2)) + - std::pow(state(2, 0), Scalar(2))) - - _tmp11 * (_tmp5 - _tmp6)); - - // Output terms (2) - if (innov_var != nullptr) { - Scalar& _innov_var = (*innov_var); - - _innov_var = R - _tmp13 * (-P(0, 0) * _tmp13 - P(2, 0) * _tmp14) - - _tmp14 * (-P(0, 2) * _tmp13 - P(2, 2) * _tmp14); - } - - if (H != nullptr) { - matrix::Matrix& _h = (*H); - - _h.setZero(); - - _h(0, 0) = -_tmp13; - _h(2, 0) = -_tmp14; - } -} // NOLINT(readability/fn_size) - -// NOLINTNEXTLINE(readability/fn_size) -} // namespace sym 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 deleted file mode 100644 index 3677dbac87c3..000000000000 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h.h +++ /dev/null @@ -1,76 +0,0 @@ -// ----------------------------------------------------------------------------- -// This file was autogenerated by symforce from template: -// function/FUNCTION.h.jinja -// Do NOT modify by hand. -// ----------------------------------------------------------------------------- - -#pragma once - -#include - -namespace sym { - -/** - * This function was autogenerated from a symbolic function. Do not modify by hand. - * - * Symbolic function: compute_yaw_321_innov_var_and_h - * - * Args: - * state: Matrix24_1 - * P: Matrix23_23 - * R: Scalar - * epsilon: Scalar - * - * Outputs: - * innov_var: Scalar - * H: Matrix23_1 - */ -template -void ComputeYaw321InnovVarAndH(const matrix::Matrix& state, - const matrix::Matrix& P, const Scalar R, - const Scalar epsilon, Scalar* const innov_var = nullptr, - matrix::Matrix* const H = nullptr) { - // Total ops: 53 - - // Input arrays - - // Intermediate terms (15) - const Scalar _tmp0 = 2 * state(0, 0); - const Scalar _tmp1 = _tmp0 * state(3, 0); - const Scalar _tmp2 = 2 * state(1, 0); - const Scalar _tmp3 = _tmp2 * state(2, 0); - const Scalar _tmp4 = _tmp1 + _tmp3; - const Scalar _tmp5 = std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp6 = std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp7 = -2 * _tmp5 - 2 * _tmp6 + 1; - const Scalar _tmp8 = _tmp7 + epsilon * ((((_tmp7) > 0) - ((_tmp7) < 0)) + Scalar(0.5)); - const Scalar _tmp9 = std::pow(_tmp8, Scalar(2)); - const Scalar _tmp10 = _tmp4 / _tmp9; - const Scalar _tmp11 = Scalar(1.0) / (_tmp8); - const Scalar _tmp12 = _tmp9 / (std::pow(_tmp4, Scalar(2)) + _tmp9); - const Scalar _tmp13 = _tmp12 * (-_tmp10 * (-_tmp1 + _tmp3) + - _tmp11 * (-_tmp5 + _tmp6 + std::pow(state(0, 0), Scalar(2)) - - std::pow(state(1, 0), Scalar(2)))); - const Scalar _tmp14 = _tmp12 * (-_tmp10 * (-_tmp0 * state(2, 0) - _tmp2 * state(3, 0)) + - _tmp11 * (_tmp2 * state(0, 0) - 2 * state(2, 0) * state(3, 0))); - - // Output terms (2) - if (innov_var != nullptr) { - Scalar& _innov_var = (*innov_var); - - _innov_var = R + _tmp13 * (P(1, 2) * _tmp14 + P(2, 2) * _tmp13) + - _tmp14 * (P(1, 1) * _tmp14 + P(2, 1) * _tmp13); - } - - if (H != nullptr) { - matrix::Matrix& _h = (*H); - - _h.setZero(); - - _h(1, 0) = _tmp14; - _h(2, 0) = _tmp13; - } -} // NOLINT(readability/fn_size) - -// NOLINTNEXTLINE(readability/fn_size) -} // namespace sym 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 deleted file mode 100644 index 3f1876623740..000000000000 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h_alternate.h +++ /dev/null @@ -1,76 +0,0 @@ -// ----------------------------------------------------------------------------- -// This file was autogenerated by symforce from template: -// function/FUNCTION.h.jinja -// Do NOT modify by hand. -// ----------------------------------------------------------------------------- - -#pragma once - -#include - -namespace sym { - -/** - * This function was autogenerated from a symbolic function. Do not modify by hand. - * - * Symbolic function: compute_yaw_321_innov_var_and_h_alternate - * - * Args: - * state: Matrix24_1 - * P: Matrix23_23 - * R: Scalar - * epsilon: Scalar - * - * Outputs: - * innov_var: Scalar - * H: Matrix23_1 - */ -template -void ComputeYaw321InnovVarAndHAlternate(const matrix::Matrix& state, - const matrix::Matrix& P, const Scalar R, - const Scalar epsilon, Scalar* const innov_var = nullptr, - matrix::Matrix* const H = nullptr) { - // Total ops: 57 - - // Input arrays - - // Intermediate terms (15) - const Scalar _tmp0 = 2 * state(2, 0); - const Scalar _tmp1 = 2 * state(1, 0); - const Scalar _tmp2 = 2 * state(0, 0) * state(3, 0); - const Scalar _tmp3 = _tmp1 * state(2, 0); - const Scalar _tmp4 = _tmp2 + _tmp3; - const Scalar _tmp5 = _tmp4 + epsilon * ((((_tmp4) > 0) - ((_tmp4) < 0)) + Scalar(0.5)); - const Scalar _tmp6 = Scalar(1.0) / (_tmp5); - const Scalar _tmp7 = std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp8 = std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp9 = -2 * _tmp7 - 2 * _tmp8 + 1; - const Scalar _tmp10 = std::pow(_tmp5, Scalar(2)); - const Scalar _tmp11 = _tmp9 / _tmp10; - const Scalar _tmp12 = _tmp10 / (_tmp10 + std::pow(_tmp9, Scalar(2))); - const Scalar _tmp13 = _tmp12 * (-_tmp11 * (-_tmp0 * state(3, 0) + _tmp1 * state(0, 0)) + - _tmp6 * (-_tmp0 * state(0, 0) - _tmp1 * state(3, 0))); - const Scalar _tmp14 = _tmp12 * (-_tmp11 * (-_tmp7 + _tmp8 + std::pow(state(0, 0), Scalar(2)) - - std::pow(state(1, 0), Scalar(2))) + - _tmp6 * (-_tmp2 + _tmp3)); - - // Output terms (2) - if (innov_var != nullptr) { - Scalar& _innov_var = (*innov_var); - - _innov_var = R - _tmp13 * (-P(1, 1) * _tmp13 - P(2, 1) * _tmp14) - - _tmp14 * (-P(1, 2) * _tmp13 - P(2, 2) * _tmp14); - } - - if (H != nullptr) { - matrix::Matrix& _h = (*H); - - _h.setZero(); - - _h(1, 0) = -_tmp13; - _h(2, 0) = -_tmp14; - } -} // NOLINT(readability/fn_size) - -// NOLINTNEXTLINE(readability/fn_size) -} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_innov_var_and_h.h new file mode 100644 index 000000000000..443fb0b03c5e --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_innov_var_and_h.h @@ -0,0 +1,65 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +namespace sym { + +/** + * This function was autogenerated from a symbolic function. Do not modify by hand. + * + * Symbolic function: compute_yaw_innov_var_and_h + * + * Args: + * state: Matrix24_1 + * P: Matrix23_23 + * R: Scalar + * + * Outputs: + * innov_var: Scalar + * H: Matrix23_1 + */ +template +void ComputeYawInnovVarAndH(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar R, + Scalar* const innov_var = nullptr, + matrix::Matrix* const H = nullptr) { + // Total ops: 36 + + // Input arrays + + // Intermediate terms (5) + const Scalar _tmp0 = 2 * state(2, 0); + const Scalar _tmp1 = 2 * state(1, 0); + const Scalar _tmp2 = -_tmp0 * state(0, 0) + _tmp1 * state(3, 0); + const Scalar _tmp3 = _tmp0 * state(3, 0) + _tmp1 * state(0, 0); + const Scalar _tmp4 = std::pow(state(0, 0), Scalar(2)) - std::pow(state(1, 0), Scalar(2)) - + std::pow(state(2, 0), Scalar(2)) + std::pow(state(3, 0), Scalar(2)); + + // Output terms (2) + if (innov_var != nullptr) { + Scalar& _innov_var = (*innov_var); + + _innov_var = R + _tmp2 * (P(0, 0) * _tmp2 + P(1, 0) * _tmp3 + P(2, 0) * _tmp4) + + _tmp3 * (P(0, 1) * _tmp2 + P(1, 1) * _tmp3 + P(2, 1) * _tmp4) + + _tmp4 * (P(0, 2) * _tmp2 + P(1, 2) * _tmp3 + P(2, 2) * _tmp4); + } + + if (H != nullptr) { + matrix::Matrix& _h = (*H); + + _h.setZero(); + + _h(0, 0) = _tmp2; + _h(1, 0) = _tmp3; + _h(2, 0) = _tmp4; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/EKF/python/tuning_tools/baro_static_pressure_compensation/baro_static_pressure_compensation_tuning.py b/src/modules/ekf2/EKF/python/tuning_tools/baro_static_pressure_compensation/baro_static_pressure_compensation_tuning.py index c1035a4bf6ec..01fe5e758ca9 100644 --- a/src/modules/ekf2/EKF/python/tuning_tools/baro_static_pressure_compensation/baro_static_pressure_compensation_tuning.py +++ b/src/modules/ekf2/EKF/python/tuning_tools/baro_static_pressure_compensation/baro_static_pressure_compensation_tuning.py @@ -44,7 +44,7 @@ import numpy as np import quaternion from scipy import optimize -from scipy.signal import detrend +from scipy.signal import sosfilt, butter def getAllData(logfile): log = ULog(logfile) @@ -59,9 +59,6 @@ def getAllData(logfile): baro = getData(log, 'vehicle_air_data', 'baro_alt_meter') t_baro = ms2s(getData(log, 'vehicle_air_data', 'timestamp')) - baro_bias = getData(log, 'estimator_baro_bias', 'bias') - t_baro_bias = ms2s(getData(log, 'estimator_baro_bias', 'timestamp')) - q = np.matrix([getData(log, 'vehicle_attitude', 'q[0]'), getData(log, 'vehicle_attitude', 'q[1]'), getData(log, 'vehicle_attitude', 'q[2]'), @@ -71,18 +68,17 @@ def getAllData(logfile): gnss_h = getData(log, 'vehicle_gps_position', 'altitude_msl_m') t_gnss = ms2s(getData(log, 'vehicle_gps_position', 'timestamp')) - (t_aligned, v_body_aligned, baro_aligned, v_local_z_aligned, gnss_h_aligned, baro_bias_aligned) = alignData(t_local, v_local, dist_bottom, t_q, q, baro, t_baro, t_gnss, gnss_h, t_baro_bias, baro_bias) + (t_aligned, v_body_aligned, baro_aligned, v_local_z_aligned, gnss_h_aligned) = alignData(t_local, v_local, dist_bottom, t_q, q, baro, t_baro, t_gnss, gnss_h) t_aligned -= t_aligned[0] - return (t_aligned, v_body_aligned, baro_aligned, v_local_z_aligned, gnss_h_aligned, baro_bias_aligned) + return (t_aligned, v_body_aligned, baro_aligned, v_local_z_aligned, gnss_h_aligned) -def alignData(t_local, v_local, dist_bottom, t_q, q, baro, t_baro, t_gnss, gnss_h, t_baro_bias, baro_bias): +def alignData(t_local, v_local, dist_bottom, t_q, q, baro, t_baro, t_gnss, gnss_h): #TODO: use resample? len_q = len(t_q) len_l = len(t_local) len_g = len(t_gnss) - len_bb = len(t_baro_bias) i_q = 0 i_l = 0 i_g = 0 @@ -91,7 +87,6 @@ def alignData(t_local, v_local, dist_bottom, t_q, q, baro, t_baro, t_gnss, gnss_ baro_aligned = [] gnss_h_aligned = [] v_local_z_aligned = [] - baro_bias_aligned = [] t_aligned = [] for i_b in range(len(t_baro)): @@ -102,8 +97,6 @@ def alignData(t_local, v_local, dist_bottom, t_q, q, baro, t_baro, t_gnss, gnss_ i_q += 1 while t_gnss[i_g] < t and i_g < len_g-1: i_g += 1 - while t_baro_bias[i_bb] < t and i_bb < len_bb-1: - i_bb += 1 # Only use in air data if dist_bottom[i_l] < 1.0: @@ -118,10 +111,9 @@ def alignData(t_local, v_local, dist_bottom, t_q, q, baro, t_baro, t_gnss, gnss_ baro_aligned = np.append(baro_aligned, baro[i_b]) v_local_z_aligned = np.append(v_local_z_aligned, v_local[2, i_l]) gnss_h_aligned = np.append(gnss_h_aligned, gnss_h[i_g]) - baro_bias_aligned = np.append(baro_bias_aligned, baro_bias[i_bb]) t_aligned.append(t) - return (t_aligned, v_body_aligned, baro_aligned, v_local_z_aligned, gnss_h_aligned, baro_bias_aligned) + return (t_aligned, v_body_aligned, baro_aligned, v_local_z_aligned, gnss_h_aligned) def getData(log, topic_name, variable_name, instance=0): variable_data = np.array([]) @@ -159,15 +151,19 @@ def baroCorrection(x, v_body): return correction def run(logfile): - (t, v_body, baro, v_local_z, gnss_h, baro_bias) = getAllData(logfile) + (t, v_body, baro, v_local_z, gnss_h) = getAllData(logfile) # x[0]: pcoef_xn / g # x[1]: pcoef_xp / g # x[2]: pcoef_yn / g # x[3]: pcoef_yp / g # x[4]: pcoef_z / g - baro -= baro_bias - baro_error = detrend(gnss_h - baro) + baro_error = (gnss_h - baro) + + # Remove low ferquency part of the signal as we're only interested in the short-term errors + baro_error -= baro_error[0] + sos = butter(4, 0.01, 'hp', fs=1/(t[1]-t[0]), output='sos') + baro_error = sosfilt(sos, baro_error) J = lambda x: np.sum(np.power(baro_error - baroCorrection(x, v_body), 2.0)) # cost function @@ -175,7 +171,7 @@ def run(logfile): res = optimize.minimize(J, x0, method='nelder-mead', options={'disp': True}) # Convert results to parameters - g = 9.81 + g = 9.80665 pcoef_xn = res.x[0] * g pcoef_xp = res.x[1] * g pcoef_yn = res.x[2] * g diff --git a/src/modules/ekf2/EKF/python/tuning_tools/mc_wind_estimator/mc_wind_estimator_tuning.py b/src/modules/ekf2/EKF/python/tuning_tools/mc_wind_estimator/mc_wind_estimator_tuning.py index 61b746fb3886..c0047a00f3ed 100644 --- a/src/modules/ekf2/EKF/python/tuning_tools/mc_wind_estimator/mc_wind_estimator_tuning.py +++ b/src/modules/ekf2/EKF/python/tuning_tools/mc_wind_estimator/mc_wind_estimator_tuning.py @@ -45,14 +45,20 @@ import quaternion from scipy import optimize -def getAllData(logfile): +def getAllData(logfile, use_gnss): log = ULog(logfile) - v_local = np.matrix([getData(log, 'vehicle_local_position', 'vx'), - getData(log, 'vehicle_local_position', 'vy'), - getData(log, 'vehicle_local_position', 'vz')]) + if use_gnss: + v_local = np.array([getData(log, 'vehicle_gps_position', 'vel_n_m_s'), + getData(log, 'vehicle_gps_position', 'vel_e_m_s'), + getData(log, 'vehicle_gps_position', 'vel_d_m_s')]) + t_v_local = ms2s(getData(log, 'vehicle_gps_position', 'timestamp')) - t_v_local = ms2s(getData(log, 'vehicle_local_position', 'timestamp')) + else: + v_local = np.array([getData(log, 'vehicle_local_position', 'vx'), + getData(log, 'vehicle_local_position', 'vy'), + getData(log, 'vehicle_local_position', 'vz')]) + t_v_local = ms2s(getData(log, 'vehicle_local_position', 'timestamp')) accel = np.matrix([getData(log, 'sensor_combined', 'accelerometer_m_s2[0]'), getData(log, 'sensor_combined', 'accelerometer_m_s2[1]'), @@ -126,8 +132,8 @@ def getData(log, topic_name, variable_name, instance=0): def ms2s(time_ms): return time_ms * 1e-6 -def run(logfile): - (t, v_body, a_body) = getAllData(logfile) +def run(logfile, use_gnss): + (t, v_body, a_body) = getAllData(logfile, use_gnss) rho = 1.15 # air densitiy rho15 = 1.225 # air density at 15 degC @@ -197,8 +203,10 @@ def run(logfile): # Provide parameter file path and name parser.add_argument('logfile', help='Full ulog file path, name and extension', type=str) + parser.add_argument('--gnss', help='Use GNSS velocity instead of local velocity estimate', + action='store_true') args = parser.parse_args() logfile = os.path.abspath(args.logfile) # Convert to absolute path - run(logfile) + run(logfile, args.gnss) diff --git a/src/modules/ekf2/EKF/sideslip_fusion.cpp b/src/modules/ekf2/EKF/sideslip_fusion.cpp index e806d13bfc5a..c35501f241c9 100644 --- a/src/modules/ekf2/EKF/sideslip_fusion.cpp +++ b/src/modules/ekf2/EKF/sideslip_fusion.cpp @@ -142,7 +142,7 @@ void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip) K.slice(State::wind_vel.idx, 0) = K_wind; } - const bool is_fused = measurementUpdate(K, sideslip.innovation_variance, sideslip.innovation); + const bool is_fused = measurementUpdate(K, H, sideslip.observation_variance, sideslip.innovation); sideslip.fused = is_fused; _fault_status.flags.bad_sideslip = !is_fused; diff --git a/src/modules/ekf2/EKF/vel_pos_fusion.cpp b/src/modules/ekf2/EKF/vel_pos_fusion.cpp deleted file mode 100644 index e615dc2d208a..000000000000 --- a/src/modules/ekf2/EKF/vel_pos_fusion.cpp +++ /dev/null @@ -1,303 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2015-2023 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file vel_pos_fusion.cpp - * - * @author Roman Bast - * @author Siddharth Bharat Purohit - * @author Paul Riseborough - * - */ - -#include -#include "ekf.h" - -void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, - const float innov_gate, estimator_aid_source2d_s &aid_src) const -{ - resetEstimatorAidStatus(aid_src); - - for (int i = 0; i < 2; i++) { - aid_src.observation[i] = obs(i); - aid_src.innovation[i] = _state.vel(i) - aid_src.observation[i]; - - aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i)); - const int state_index = State::vel.idx + i; - aid_src.innovation_variance[i] = P(state_index, state_index) + aid_src.observation_variance[i]; - } - - setEstimatorAidStatusTestRatio(aid_src, innov_gate); - - aid_src.timestamp_sample = time_us; -} - -void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &obs, const Vector3f &obs_var, - const float innov_gate, estimator_aid_source3d_s &aid_src) const -{ - resetEstimatorAidStatus(aid_src); - - for (int i = 0; i < 3; i++) { - aid_src.observation[i] = obs(i); - aid_src.innovation[i] = _state.vel(i) - aid_src.observation[i]; - - aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i)); - const int state_index = State::vel.idx + i; - aid_src.innovation_variance[i] = P(state_index, state_index) + aid_src.observation_variance[i]; - } - - setEstimatorAidStatusTestRatio(aid_src, innov_gate); - - // vz special case if there is bad vertical acceleration data, then don't reject measurement, - // but limit innovation to prevent spikes that could destabilise the filter - if (_fault_status.flags.bad_acc_vertical && aid_src.innovation_rejected) { - const float innov_limit = innov_gate * sqrtf(aid_src.innovation_variance[2]); - aid_src.innovation[2] = math::constrain(aid_src.innovation[2], -innov_limit, innov_limit); - aid_src.innovation_rejected = false; - } - - aid_src.timestamp_sample = time_us; -} - -void Ekf::updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const float obs, const float obs_var, - const float innov_gate, estimator_aid_source1d_s &aid_src) const -{ - resetEstimatorAidStatus(aid_src); - - aid_src.observation = obs; - aid_src.innovation = _state.pos(2) - aid_src.observation; - - aid_src.observation_variance = math::max(sq(0.01f), obs_var); - aid_src.innovation_variance = P(State::pos.idx + 2, State::pos.idx + 2) + aid_src.observation_variance; - - setEstimatorAidStatusTestRatio(aid_src, innov_gate); - - // z special case if there is bad vertical acceleration data, then don't reject measurement, - // but limit innovation to prevent spikes that could destabilise the filter - if (_fault_status.flags.bad_acc_vertical && aid_src.innovation_rejected) { - const float innov_limit = innov_gate * sqrtf(aid_src.innovation_variance); - aid_src.innovation = math::constrain(aid_src.innovation, -innov_limit, innov_limit); - aid_src.innovation_rejected = false; - } - - aid_src.timestamp_sample = time_us; -} - -void Ekf::updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, - const float innov_gate, estimator_aid_source2d_s &aid_src) const -{ - resetEstimatorAidStatus(aid_src); - - for (int i = 0; i < 2; i++) { - aid_src.observation[i] = obs(i); - aid_src.innovation[i] = _state.pos(i) - aid_src.observation[i]; - - aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i)); - const int state_index = State::pos.idx + i; - aid_src.innovation_variance[i] = P(state_index, state_index) + aid_src.observation_variance[i]; - } - - setEstimatorAidStatusTestRatio(aid_src, innov_gate); - - aid_src.timestamp_sample = time_us; -} - -void Ekf::fuseVelocity(estimator_aid_source2d_s &aid_src) -{ - if (!aid_src.innovation_rejected) { - // vx, vy - 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; - - } else { - aid_src.fused = false; - } - } -} - -void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src) -{ - if (!aid_src.innovation_rejected) { - // vx, vy, vz - 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; - - } else { - aid_src.fused = false; - } - } -} - -void Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src) -{ - // x & y - 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; - - } else { - aid_src.fused = false; - } - } -} - -void Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src) -{ - // z - 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; - } - } -} - -// Helper function that fuses a single velocity or position measurement -bool Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int state_index) -{ - 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 < State::size; row++) { - Kfusion(row) = P(row, state_index) / innov_var; - } - - clearInhibitedStateKalmanGains(Kfusion); - - SquareMatrixState KHP; - - 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(state_index, healthy); - - if (healthy) { - // apply the covariance corrections - P -= KHP; - - fixCovarianceErrors(true); - - // apply the state corrections - fuse(Kfusion, innov); - - return true; - } - - return false; -} - -void Ekf::setVelPosStatus(const int state_index, const bool healthy) -{ - switch (state_index) { - case State::vel.idx: - if (healthy) { - _fault_status.flags.bad_vel_N = false; - _time_last_hor_vel_fuse = _time_delayed_us; - - } else { - _fault_status.flags.bad_vel_N = true; - } - - break; - - case State::vel.idx + 1: - if (healthy) { - _fault_status.flags.bad_vel_E = false; - _time_last_hor_vel_fuse = _time_delayed_us; - - } else { - _fault_status.flags.bad_vel_E = true; - } - - break; - - case State::vel.idx + 2: - if (healthy) { - _fault_status.flags.bad_vel_D = false; - _time_last_ver_vel_fuse = _time_delayed_us; - - } else { - _fault_status.flags.bad_vel_D = true; - } - - break; - - case State::pos.idx: - if (healthy) { - _fault_status.flags.bad_pos_N = false; - _time_last_hor_pos_fuse = _time_delayed_us; - - } else { - _fault_status.flags.bad_pos_N = true; - } - - break; - - case State::pos.idx + 1: - if (healthy) { - _fault_status.flags.bad_pos_E = false; - _time_last_hor_pos_fuse = _time_delayed_us; - - } else { - _fault_status.flags.bad_pos_E = true; - } - - break; - - case State::pos.idx + 2: - if (healthy) { - _fault_status.flags.bad_pos_D = false; - _time_last_hgt_fuse = _time_delayed_us; - - } else { - _fault_status.flags.bad_pos_D = true; - } - - break; - } -} diff --git a/src/modules/ekf2/EKF/velocity_fusion.cpp b/src/modules/ekf2/EKF/velocity_fusion.cpp new file mode 100644 index 000000000000..fbb079c1fdee --- /dev/null +++ b/src/modules/ekf2/EKF/velocity_fusion.cpp @@ -0,0 +1,195 @@ +/**************************************************************************** + * + * Copyright (c) 2015-2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "ekf.h" + +void Ekf::updateHorizontalVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, + const float innov_gate, estimator_aid_source2d_s &aid_src) const +{ + resetEstimatorAidStatus(aid_src); + + for (int i = 0; i < 2; i++) { + aid_src.observation[i] = obs(i); + aid_src.innovation[i] = _state.vel(i) - aid_src.observation[i]; + + aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i)); + const int state_index = State::vel.idx + i; + aid_src.innovation_variance[i] = P(state_index, state_index) + aid_src.observation_variance[i]; + } + + setEstimatorAidStatusTestRatio(aid_src, innov_gate); + + aid_src.timestamp_sample = time_us; +} + +void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &obs, const Vector3f &obs_var, + const float innov_gate, estimator_aid_source3d_s &aid_src) const +{ + resetEstimatorAidStatus(aid_src); + + for (int i = 0; i < 3; i++) { + aid_src.observation[i] = obs(i); + aid_src.innovation[i] = _state.vel(i) - aid_src.observation[i]; + + aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i)); + const int state_index = State::vel.idx + i; + aid_src.innovation_variance[i] = P(state_index, state_index) + aid_src.observation_variance[i]; + } + + setEstimatorAidStatusTestRatio(aid_src, innov_gate); + + // vz special case if there is bad vertical acceleration data, then don't reject measurement, + // but limit innovation to prevent spikes that could destabilise the filter + if (_fault_status.flags.bad_acc_vertical && aid_src.innovation_rejected) { + const float innov_limit = innov_gate * sqrtf(aid_src.innovation_variance[2]); + aid_src.innovation[2] = math::constrain(aid_src.innovation[2], -innov_limit, innov_limit); + aid_src.innovation_rejected = false; + } + + aid_src.timestamp_sample = time_us; +} + +void Ekf::fuseHorizontalVelocity(estimator_aid_source2d_s &aid_src) +{ + // vx, vy + if (!aid_src.innovation_rejected + && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0], State::vel.idx + 0) + && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1], State::vel.idx + 1) + ) { + aid_src.fused = true; + aid_src.time_last_fuse = _time_delayed_us; + + _time_last_hor_vel_fuse = _time_delayed_us; + + } else { + aid_src.fused = false; + } +} + +void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src) +{ + // vx, vy, vz + if (!aid_src.innovation_rejected + && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0], State::vel.idx + 0) + && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1], State::vel.idx + 1) + && fuseDirectStateMeasurement(aid_src.innovation[2], aid_src.innovation_variance[2], aid_src.observation_variance[2], State::vel.idx + 2) + ) { + aid_src.fused = true; + aid_src.time_last_fuse = _time_delayed_us; + + _time_last_hor_vel_fuse = _time_delayed_us; + _time_last_ver_vel_fuse = _time_delayed_us; + + } else { + aid_src.fused = false; + } +} + +void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f &new_horz_vel_var) +{ + const Vector2f delta_horz_vel = new_horz_vel - Vector2f(_state.vel); + _state.vel.xy() = new_horz_vel; + + if (PX4_ISFINITE(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>(State::vel.idx + 1, math::max(sq(0.01f), new_horz_vel_var(1))); + } + + _output_predictor.resetHorizontalVelocityTo(delta_horz_vel); + + // record the state change + if (_state_reset_status.reset_count.velNE == _state_reset_count_prev.velNE) { + _state_reset_status.velNE_change = delta_horz_vel; + + } else { + // there's already a reset this update, accumulate total delta + _state_reset_status.velNE_change += delta_horz_vel; + } + + _state_reset_status.reset_count.velNE++; + + // Reset the timout timer + _time_last_hor_vel_fuse = _time_delayed_us; +} + +void Ekf::resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var) +{ + const float delta_vert_vel = new_vert_vel - _state.vel(2); + _state.vel(2) = new_vert_vel; + + if (PX4_ISFINITE(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); + + // record the state change + if (_state_reset_status.reset_count.velD == _state_reset_count_prev.velD) { + _state_reset_status.velD_change = delta_vert_vel; + + } else { + // there's already a reset this update, accumulate total delta + _state_reset_status.velD_change += delta_vert_vel; + } + + _state_reset_status.reset_count.velD++; + + // Reset the timout timer + _time_last_ver_vel_fuse = _time_delayed_us; +} + +void Ekf::resetHorizontalVelocityToZero() +{ + ECL_INFO("reset velocity to zero"); + _information_events.flags.reset_vel_to_zero = true; + + // Used when falling back to non-aiding mode of operation + resetHorizontalVelocityTo(Vector2f{0.f, 0.f}, 25.f); +} + +void Ekf::resetVerticalVelocityToZero() +{ + // we don't know what the vertical velocity is, so set it to zero + // Set the variance to a value large enough to allow the state to converge quickly + // that does not destabilise the filter + resetVerticalVelocityTo(0.0f, 10.f); +} + +void Ekf::resetVelocityTo(const Vector3f &new_vel, const Vector3f &new_vel_var) +{ + resetHorizontalVelocityTo(Vector2f(new_vel), Vector2f(new_vel_var(0), new_vel_var(1))); + resetVerticalVelocityTo(new_vel(2), new_vel_var(2)); +} diff --git a/src/modules/ekf2/EKF/yaw_fusion.cpp b/src/modules/ekf2/EKF/yaw_fusion.cpp index ddab6ee6343e..80ce35210b14 100644 --- a/src/modules/ekf2/EKF/yaw_fusion.cpp +++ b/src/modules/ekf2/EKF/yaw_fusion.cpp @@ -33,8 +33,7 @@ #include "ekf.h" -#include -#include +#include #include @@ -42,7 +41,7 @@ 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); + sym::ComputeYawInnovVarAndH(_state.vector(), P, aid_src_status.observation_variance, &aid_src_status.innovation_variance, &H_YAW); return fuseYaw(aid_src_status, H_YAW); } @@ -113,7 +112,7 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H _innov_check_fail_status.flags.reject_yaw = false; } - if (measurementUpdate(Kfusion, aid_src_status.innovation_variance, aid_src_status.innovation)) { + if (measurementUpdate(Kfusion, H_YAW, aid_src_status.observation_variance, aid_src_status.innovation)) { _time_last_heading_fuse = _time_delayed_us; @@ -135,10 +134,5 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H void Ekf::computeYawInnovVarAndH(float variance, float &innovation_variance, VectorState &H_YAW) const { - if (shouldUse321RotationSequence(_R_to_earth)) { - sym::ComputeYaw321InnovVarAndH(_state.vector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW); - - } else { - sym::ComputeYaw312InnovVarAndH(_state.vector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW); - } + sym::ComputeYawInnovVarAndH(_state.vector(), P, variance, &innovation_variance, &H_YAW); } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index e72bf3ce71a2..c75093711ea9 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -40,6 +40,9 @@ using matrix::Eulerf; using matrix::Quatf; using matrix::Vector3f; +static constexpr float kDefaultExternalPosAccuracy = 50.0f; // [m] +static constexpr float kMaxDelaySecondsExternalPosMeasurement = 15.0f; // [s] + pthread_mutex_t ekf2_module_mutex = PTHREAD_MUTEX_INITIALIZER; static px4::atomic _objects[EKF2_MAX_INSTANCES] {}; #if defined(CONFIG_EKF2_MULTI_INSTANCE) @@ -132,7 +135,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_decl_type(_params->mag_declination_source), _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), @@ -227,31 +229,8 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): EKF2::~EKF2() { - perf_free(_ecl_ekf_update_perf); - perf_free(_ecl_ekf_update_full_perf); + perf_free(_ekf_update_perf); perf_free(_msg_missed_imu_perf); -#if defined(CONFIG_EKF2_BAROMETER) - perf_free(_msg_missed_air_data_perf); -#endif // CONFIG_EKF2_BAROMETER -#if defined(CONFIG_EKF2_AIRSPEED) - perf_free(_msg_missed_airspeed_perf); -#endif // CONFIG_EKF2_AIRSPEED -#if defined(CONFIG_EKF2_RANGE_FINDER) - perf_free(_msg_missed_distance_sensor_perf); -#endif // CONFIG_EKF2_RANGE_FINDER -#if defined(CONFIG_EKF2_GNSS) - perf_free(_msg_missed_gps_perf); -#endif // CONFIG_EKF2_GNSS -#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); -#endif // CONFIG_EKF2_OPTICAL_FLOW } #if defined(CONFIG_EKF2_MULTI_INSTANCE) @@ -270,6 +249,7 @@ bool EKF2::multi_init(int imu, int mag) _estimator_states_pub.advertise(); _estimator_status_flags_pub.advertise(); _estimator_status_pub.advertise(); + #if defined(CONFIG_EKF2_GNSS) _yaw_est_pub.advertise(); #endif // CONFIG_EKF2_GNSS @@ -333,6 +313,14 @@ bool EKF2::multi_init(int imu, int mag) # endif // CONFIG_EKF2_GNSS_YAW #endif // CONFIG_EKF2_GNSS +#if defined(CONFIG_EKF2_GRAVITY_FUSION) + + if (_param_ekf2_imu_ctrl.get() & static_cast(ImuCtrl::GravityVector)) { + _estimator_aid_src_gravity_pub.advertise(); + } + +#endif // CONFIG_EKF2_GRAVITY_FUSION + #if defined(CONFIG_EKF2_RANGE_FINDER) // RNG advertise @@ -392,41 +380,20 @@ bool EKF2::multi_init(int imu, int mag) } #endif // CONFIG_EKF2_MULTI_INSTANCE -int EKF2::print_status() +int EKF2::print_status(bool verbose) { PX4_INFO_RAW("ekf2:%d EKF dt: %.4fs, attitude: %d, local position: %d, global position: %d\n", _instance, (double)_ekf.get_dt_ekf_avg(), _ekf.attitude_valid(), _ekf.local_position_is_valid(), _ekf.global_position_is_valid()); - perf_print_counter(_ecl_ekf_update_perf); - perf_print_counter(_ecl_ekf_update_full_perf); + perf_print_counter(_ekf_update_perf); perf_print_counter(_msg_missed_imu_perf); -#if defined(CONFIG_EKF2_BAROMETER) - perf_print_counter(_msg_missed_air_data_perf); -#endif // CONFIG_EKF2_BAROMETER -#if defined(CONFIG_EKF2_AIRSPEED) - perf_print_counter(_msg_missed_airspeed_perf); -#endif // CONFIG_EKF2_AIRSPEED -#if defined(CONFIG_EKF2_RANGE_FINDER) - perf_print_counter(_msg_missed_distance_sensor_perf); -#endif // CONFIG_EKF2_RANGE_FINDER -#if defined(CONFIG_EKF2_GNSS) - perf_print_counter(_msg_missed_gps_perf); -#endif // CONFIG_EKF2_GNSS -#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); -#endif // CONFIG_EKF2_OPTICAL_FLOW -#if defined(DEBUG_BUILD) - _ekf.print_status(); -#endif // DEBUG_BUILD + if (verbose) { +#if defined(CONFIG_EKF2_VERBOSE_STATUS) + _ekf.print_status(); +#endif // CONFIG_EKF2_VERBOSE_STATUS + } return 0; } @@ -554,6 +521,26 @@ void EKF2::Run() _instance, latitude, longitude, static_cast(altitude)); } } + + if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE) { + if ((_ekf.control_status_flags().wind_dead_reckoning || _ekf.control_status_flags().inertial_dead_reckoning) && + PX4_ISFINITE(vehicle_command.param2) && PX4_ISFINITE(vehicle_command.param5) && PX4_ISFINITE(vehicle_command.param6)) { + + const float measurement_delay_seconds = math::constrain(vehicle_command.param2, 0.0f, + kMaxDelaySecondsExternalPosMeasurement); + const uint64_t timestamp_observation = vehicle_command.timestamp - measurement_delay_seconds * 1_s; + + float accuracy = kDefaultExternalPosAccuracy; + + if (PX4_ISFINITE(vehicle_command.param3) && vehicle_command.param3 > FLT_EPSILON) { + accuracy = vehicle_command.param3; + } + + // TODO add check for lat and long validity + _ekf.resetGlobalPosToExternalObservation(vehicle_command.param5, vehicle_command.param6, + accuracy, timestamp_observation); + } + } } } @@ -757,7 +744,7 @@ void EKF2::Run() const hrt_abstime ekf_update_start = hrt_absolute_time(); if (_ekf.update()) { - perf_set_elapsed(_ecl_ekf_update_full_perf, hrt_elapsed_time(&ekf_update_start)); + perf_set_elapsed(_ekf_update_perf, hrt_elapsed_time(&ekf_update_start)); PublishLocalPosition(now); PublishOdometry(now, imu_sample_new); @@ -805,10 +792,6 @@ void EKF2::Run() #if defined(CONFIG_EKF2_MAGNETOMETER) UpdateMagCalibration(now); #endif // CONFIG_EKF2_MAGNETOMETER - - } else { - // ekf no update - perf_set_elapsed(_ecl_ekf_update_perf, hrt_elapsed_time(&ekf_update_start)); } // publish ekf2_timestamps @@ -1597,8 +1580,10 @@ void EKF2::PublishLocalPosition(const hrt_abstime ×tamp) lpos.heading = Eulerf(_ekf.getQuaternion()).psi(); lpos.unaided_heading = _ekf.getUnaidedYaw(); + lpos.heading_var = _ekf.getYawVar(); lpos.delta_heading = Eulerf(delta_q_reset).psi(); lpos.heading_good_for_control = _ekf.isYawFinalAlignComplete(); + lpos.tilt_var = _ekf.getTiltVariance(); #if defined(CONFIG_EKF2_TERRAIN) // Distance to bottom surface (ground) in meters, must be positive @@ -1910,12 +1895,6 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) status_flags.fs_bad_sideslip = _ekf.fault_status_flags().bad_sideslip; status_flags.fs_bad_optflow_x = _ekf.fault_status_flags().bad_optflow_X; status_flags.fs_bad_optflow_y = _ekf.fault_status_flags().bad_optflow_Y; - status_flags.fs_bad_vel_n = _ekf.fault_status_flags().bad_vel_N; - status_flags.fs_bad_vel_e = _ekf.fault_status_flags().bad_vel_E; - status_flags.fs_bad_vel_d = _ekf.fault_status_flags().bad_vel_D; - status_flags.fs_bad_pos_n = _ekf.fault_status_flags().bad_pos_N; - status_flags.fs_bad_pos_e = _ekf.fault_status_flags().bad_pos_E; - status_flags.fs_bad_pos_d = _ekf.fault_status_flags().bad_pos_D; status_flags.fs_bad_acc_bias = _ekf.fault_status_flags().bad_acc_bias; status_flags.fs_bad_acc_vertical = _ekf.fault_status_flags().bad_acc_vertical; status_flags.fs_bad_acc_clipping = _ekf.fault_status_flags().bad_acc_clipping; @@ -2051,16 +2030,9 @@ void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps) // EKF airspeed sample // prefer ORB_ID(airspeed_validated) if available, otherwise fallback to raw airspeed ORB_ID(airspeed) if (_airspeed_validated_sub.updated()) { - const unsigned last_generation = _airspeed_validated_sub.get_last_generation(); airspeed_validated_s airspeed_validated; if (_airspeed_validated_sub.update(&airspeed_validated)) { - if (_msg_missed_airspeed_validated_perf == nullptr) { - _msg_missed_airspeed_validated_perf = perf_alloc(PC_COUNT, MODULE_NAME": airspeed validated messages missed"); - - } else if (_airspeed_validated_sub.get_last_generation() != last_generation + 1) { - perf_count(_msg_missed_airspeed_validated_perf); - } if (PX4_ISFINITE(airspeed_validated.true_airspeed_m_s) && PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s) @@ -2080,17 +2052,9 @@ void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps) } else if (((ekf2_timestamps.timestamp - _airspeed_validated_timestamp_last) > 3_s) && _airspeed_sub.updated()) { // use ORB_ID(airspeed) if ORB_ID(airspeed_validated) is unavailable - const unsigned last_generation = _airspeed_sub.get_last_generation(); airspeed_s airspeed; if (_airspeed_sub.update(&airspeed)) { - if (_msg_missed_airspeed_perf == nullptr) { - _msg_missed_airspeed_perf = perf_alloc(PC_COUNT, MODULE_NAME": airspeed messages missed"); - - } else if (_airspeed_sub.get_last_generation() != last_generation + 1) { - perf_count(_msg_missed_airspeed_perf); - } - // The airspeed measurement received via ORB_ID(airspeed) topic has not been corrected // for scale factor errors and requires the ASPD_SCALE correction to be applied. const float true_airspeed_m_s = airspeed.true_airspeed_m_s * _airspeed_scale_factor; @@ -2119,17 +2083,9 @@ void EKF2::UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps) { // EKF auxiliary velocity sample // - use the landing target pose estimate as another source of velocity data - const unsigned last_generation = _landing_target_pose_sub.get_last_generation(); landing_target_pose_s landing_target_pose; if (_landing_target_pose_sub.update(&landing_target_pose)) { - if (_msg_missed_landing_target_pose_perf == nullptr) { - _msg_missed_landing_target_pose_perf = perf_alloc(PC_COUNT, MODULE_NAME": landing_target_pose messages missed"); - - } else if (_landing_target_pose_sub.get_last_generation() != last_generation + 1) { - perf_count(_msg_missed_landing_target_pose_perf); - } - // we can only use the landing target if it has a fixed position and a valid velocity estimate if (landing_target_pose.is_static && landing_target_pose.rel_vel_valid) { // velocity of vehicle relative to target has opposite sign to target relative to vehicle @@ -2148,16 +2104,9 @@ void EKF2::UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps) void EKF2::UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps) { // EKF baro sample - const unsigned last_generation = _airdata_sub.get_last_generation(); vehicle_air_data_s airdata; if (_airdata_sub.update(&airdata)) { - if (_msg_missed_air_data_perf == nullptr) { - _msg_missed_air_data_perf = perf_alloc(PC_COUNT, MODULE_NAME": vehicle_air_data messages missed"); - - } else if (_airdata_sub.get_last_generation() != last_generation + 1) { - perf_count(_msg_missed_air_data_perf); - } bool reset = false; @@ -2195,17 +2144,10 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps) { // EKF external vision sample bool new_ev_odom = false; - const unsigned last_generation = _ev_odom_sub.get_last_generation(); vehicle_odometry_s ev_odom; if (_ev_odom_sub.update(&ev_odom)) { - if (_msg_missed_odometry_perf == nullptr) { - _msg_missed_odometry_perf = perf_alloc(PC_COUNT, MODULE_NAME": vehicle_visual_odometry messages missed"); - - } else if (_ev_odom_sub.get_last_generation() != last_generation + 1) { - perf_count(_msg_missed_odometry_perf); - } extVisionSample ev_data{}; ev_data.pos.setNaN(); @@ -2351,16 +2293,9 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps) { // EKF flow sample bool new_optical_flow = false; - const unsigned last_generation = _vehicle_optical_flow_sub.get_last_generation(); vehicle_optical_flow_s optical_flow; if (_vehicle_optical_flow_sub.update(&optical_flow)) { - if (_msg_missed_optical_flow_perf == nullptr) { - _msg_missed_optical_flow_perf = perf_alloc(PC_COUNT, MODULE_NAME": optical_flow messages missed"); - - } else if (_vehicle_optical_flow_sub.get_last_generation() != last_generation + 1) { - perf_count(_msg_missed_optical_flow_perf); - } const float dt = 1e-6f * (float)optical_flow.integration_timespan_us; Vector2f flow_rate; @@ -2424,16 +2359,9 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps) void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps) { // EKF GPS message - const unsigned last_generation = _vehicle_gps_position_sub.get_last_generation(); sensor_gps_s vehicle_gps_position; if (_vehicle_gps_position_sub.update(&vehicle_gps_position)) { - if (_msg_missed_gps_perf == nullptr) { - _msg_missed_gps_perf = perf_alloc(PC_COUNT, MODULE_NAME": vehicle_gps_position messages missed"); - - } else if (_vehicle_gps_position_sub.get_last_generation() != last_generation + 1) { - perf_count(_msg_missed_gps_perf); - } Vector3f vel_ned; @@ -2475,16 +2403,9 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps) #if defined(CONFIG_EKF2_MAGNETOMETER) void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps) { - const unsigned last_generation = _magnetometer_sub.get_last_generation(); vehicle_magnetometer_s magnetometer; if (_magnetometer_sub.update(&magnetometer)) { - if (_msg_missed_magnetometer_perf == nullptr) { - _msg_missed_magnetometer_perf = perf_alloc(PC_COUNT, MODULE_NAME": vehicle_magnetometer messages missed"); - - } else if (_magnetometer_sub.get_last_generation() != last_generation + 1) { - perf_count(_msg_missed_magnetometer_perf); - } bool reset = false; @@ -2544,7 +2465,6 @@ void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps) _distance_sensor_selected = i; _last_range_sensor_update = distance_sensor.timestamp; - _distance_sensor_last_generation = _distance_sensor_subs[_distance_sensor_selected].get_last_generation() - 1; break; } } @@ -2554,17 +2474,6 @@ void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps) if (_distance_sensor_selected >= 0 && _distance_sensor_subs[_distance_sensor_selected].update(&distance_sensor)) { // EKF range sample - - if (_msg_missed_distance_sensor_perf == nullptr) { - _msg_missed_distance_sensor_perf = perf_alloc(PC_COUNT, MODULE_NAME": distance_sensor messages missed"); - - } else if (_distance_sensor_subs[_distance_sensor_selected].get_last_generation() != _distance_sensor_last_generation + - 1) { - perf_count(_msg_missed_distance_sensor_perf); - } - - _distance_sensor_last_generation = _distance_sensor_subs[_distance_sensor_selected].get_last_generation(); - if (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING) { rangeSample range_sample { .time_us = distance_sensor.timestamp, @@ -2945,7 +2854,11 @@ timestamps from the sensor topics. PRINT_MODULE_USAGE_NAME("ekf2", "estimator"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_PARAM_FLAG('r', "Enable replay mode", true); - PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + PRINT_MODULE_USAGE_COMMAND("stop"); + PRINT_MODULE_USAGE_COMMAND_DESCR("status", "print status info"); +#if defined(CONFIG_EKF2_VERBOSE_STATUS) + PRINT_MODULE_USAGE_ARG("-v", "verbose (print all states and full covariance matrix)", true); +#endif // CONFIG_EKF2_VERBOSE_STATUS #if defined(CONFIG_EKF2_MULTI_INSTANCE) PRINT_MODULE_USAGE_COMMAND_DESCR("select_instance", "Request switch to new estimator instance"); PRINT_MODULE_USAGE_ARG("", "Specify desired estimator instance", false); @@ -3005,10 +2918,18 @@ extern "C" __EXPORT int ekf2_main(int argc, char *argv[]) } #endif // CONFIG_EKF2_MULTI_INSTANCE + bool verbose_status = false; + +#if defined(CONFIG_EKF2_VERBOSE_STATUS) + if (argc > 2 && (strcmp(argv[2], "-v") == 0)) { + verbose_status = true; + } +#endif // CONFIG_EKF2_VERBOSE_STATUS + for (int i = 0; i < EKF2_MAX_INSTANCES; i++) { if (_objects[i].load()) { PX4_INFO_RAW("\n"); - _objects[i].load()->print_status(); + _objects[i].load()->print_status(verbose_status); } } diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 060ce108dbaa..1d54315b8cc8 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -139,7 +139,7 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem /** @see ModuleBase */ static int print_usage(const char *reason = nullptr); - int print_status(); + int print_status(bool verbose = false); bool should_exit() const { return _task_should_exit.load(); } @@ -276,10 +276,8 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem uint64_t _start_time_us = 0; ///< system time at EKF start (uSec) int64_t _last_time_slip_us = 0; ///< Last time slip (uSec) - 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 _ekf_update_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": EKF update")}; perf_counter_t _msg_missed_imu_perf{perf_alloc(PC_COUNT, MODULE_NAME": IMU message missed")}; - perf_counter_t _msg_missed_odometry_perf{nullptr}; InFlightCalibration _accel_cal{}; InFlightCalibration _gyro_cal{}; @@ -301,8 +299,6 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem #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 @@ -341,8 +337,6 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem uORB::PublicationMulti _estimator_aid_src_aux_vel_pub{ORB_ID(estimator_aid_src_aux_vel)}; hrt_abstime _status_aux_vel_pub_last{0}; - - perf_counter_t _msg_missed_landing_target_pose_perf{nullptr}; #endif // CONFIG_EKF2_AUXVEL #if defined(CONFIG_EKF2_TERRAIN) @@ -365,13 +359,9 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem 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 _optical_flow_vel_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}; @@ -398,9 +388,6 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem uORB::PublicationMulti _estimator_aid_src_airspeed_pub {ORB_ID(estimator_aid_src_airspeed)}; hrt_abstime _status_airspeed_pub_last{0}; - - perf_counter_t _msg_missed_airspeed_perf{nullptr}; - perf_counter_t _msg_missed_airspeed_validated_perf{nullptr}; #endif // CONFIG_EKF2_AIRSPEED #if defined(CONFIG_EKF2_SIDESLIP) @@ -430,8 +417,6 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem uORB::SubscriptionMultiArray _distance_sensor_subs{ORB_ID::distance_sensor}; hrt_abstime _last_range_sensor_update{0}; int _distance_sensor_selected{-1}; // because we can have several distance sensor instances with different orientations - unsigned _distance_sensor_last_generation{0}; - perf_counter_t _msg_missed_distance_sensor_perf{nullptr}; #endif // CONFIG_EKF2_RANGE_FINDER bool _callback_registered{false}; @@ -473,9 +458,7 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem #endif // CONFIG_EKF2_WIND #if defined(CONFIG_EKF2_GNSS) - perf_counter_t _msg_missed_gps_perf {nullptr}; - - uint64_t _gps_time_usec{0}; + uint64_t _gps_time_usec {0}; int32_t _gps_alttitude_ellipsoid{0}; ///< altitude in 1E-3 meters (millimeters) above ellipsoid uint64_t _gps_alttitude_ellipsoid_previous_timestamp{0}; ///< storage for previous timestamp to compute dt float _wgs84_hgt_offset = 0; ///< height offset between AMSL and WGS84 @@ -622,7 +605,6 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem (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, diff --git a/src/modules/ekf2/Kconfig b/src/modules/ekf2/Kconfig index e4e94c31d10c..a1899344154f 100644 --- a/src/modules/ekf2/Kconfig +++ b/src/modules/ekf2/Kconfig @@ -4,6 +4,15 @@ menuconfig MODULES_EKF2 ---help--- Enable support for ekf2 +menuconfig EKF2_VERBOSE_STATUS +depends on MODULES_EKF2 + bool "verbose status output" + default n + depends on !BOARD_CONSTRAINED_MEMORY + depends on !BOARD_CONSTRAINED_FLASH + ---help--- + ekf2 status verbose output. + menuconfig EKF2_MULTI_INSTANCE depends on MODULES_EKF2 bool "multi-EKF support" diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index cc8adcca56d7..396ba9651559 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -62,7 +62,7 @@ PARAM_DEFINE_INT32(EKF2_PREDICT_US, 10000); * @bit 1 Accel Bias * @bit 2 Gravity vector fusion */ -PARAM_DEFINE_INT32(EKF2_IMU_CTRL, 3); +PARAM_DEFINE_INT32(EKF2_IMU_CTRL, 7); /** * Magnetometer measurement delay relative to IMU measurements @@ -503,9 +503,9 @@ PARAM_DEFINE_INT32(EKF2_DECL_TYPE, 7); PARAM_DEFINE_INT32(EKF2_MAG_TYPE, 0); /** - * Horizontal acceleration threshold used by automatic selection of magnetometer fusion method. + * Horizontal acceleration threshold used for heading observability check * - * This parameter is used when the magnetometer fusion method is set automatically (EKF2_MAG_TYPE = 0). If the filtered horizontal acceleration is greater than this parameter value, then the EKF will use 3-axis magnetometer fusion. + * The heading is assumed to be observable when the body acceleration is greater than this parameter when a global position/velocity aiding source is active. * * @group EKF2 * @min 0.0 @@ -515,19 +515,6 @@ PARAM_DEFINE_INT32(EKF2_MAG_TYPE, 0); */ PARAM_DEFINE_FLOAT(EKF2_MAG_ACCLIM, 0.5f); -/** - * Yaw rate threshold used by automatic selection of magnetometer fusion method. - * - * This parameter is used when the magnetometer fusion method is set automatically (EKF2_MAG_TYPE = 0). If the filtered yaw rate is greater than this parameter value, then the EKF will use 3-axis magnetometer fusion. - * - * @group EKF2 - * @min 0.0 - * @max 1.0 - * @unit rad/s - * @decimal 2 - */ -PARAM_DEFINE_FLOAT(EKF2_MAG_YAWLIM, 0.20f); - /** * Gate size for barometric and GPS height fusion * @@ -826,7 +813,7 @@ PARAM_DEFINE_FLOAT(EKF2_EVA_NOISE, 0.1f); * @group EKF2 * @min 0.1 * @max 10.0 - * @unit m/s^2 + * @unit g0 * @decimal 2 */ PARAM_DEFINE_FLOAT(EKF2_GRAV_NOISE, 1.0f); 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 9f5396bb7478..17437ae7216c 100644 --- a/src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv +++ b/src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv @@ -1,391 +1,391 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7],state[8],state[9],state[10],state[11],state[12],state[13],state[14],state[15],state[16],state[17],state[18],state[19],state[20],state[21],state[22],state[23],variance[0],variance[1],variance[2],variance[3],variance[4],variance[5],variance[6],variance[7],variance[8],variance[9],variance[10],variance[11],variance[12],variance[13],variance[14],variance[15],variance[16],variance[17],variance[18],variance[19],variance[20],variance[21],variance[22],variance[23] -10000,1,-0.0094,-0.01,-3.2e-06,0.00023,7.3e-05,-0.011,7.2e-06,2.2e-06,-0.00045,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0.01,0.00018,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0,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,0.01,0.01,0.00046,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -190000,1,-0.0094,-0.011,2.8e-05,6.9e-05,0.004,-0.041,-5.9e-06,0.00043,-0.0044,-3e-11,-2.7e-12,5.9e-13,0,0,-2e-09,0,0,0,0,0,0,0,0,0.011,0.011,0.00093,25,25,10,1e+02,1e+02,1,0.01,0.01,0.01,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -290000,1,-0.0094,-0.011,6.3e-05,0.001,0.0064,-0.053,3.8e-05,0.00036,-0.007,9.1e-10,1e-10,-2.8e-11,0,0,-4.8e-07,0,0,0,0,0,0,0,0,0.012,0.011,0.00077,25,25,9.6,0.37,0.37,0.41,0.01,0.01,0.0052,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -390000,1,-0.0094,-0.011,7e-05,0.0024,0.0083,-0.059,0.00021,0.0011,-0.0094,-1.1e-08,2.8e-09,7.7e-11,0,0,-4.5e-06,0,0,0,0,0,0,0,0,0.012,0.012,0.0012,25,25,8.1,0.97,0.97,0.32,0.01,0.01,0.0052,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -490000,1,-0.0095,-0.012,2e-05,0.0039,0.0048,-0.06,0.00024,0.00049,-0.011,1.6e-06,-3.7e-07,1.4e-08,0,0,-1.6e-05,0,0,0,0,0,0,0,0,0.013,0.013,0.00073,7.8,7.8,5.9,0.34,0.34,0.31,0.01,0.01,0.0024,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -590000,1,-0.0095,-0.012,2.6e-05,0.006,0.0073,-0.059,0.00074,0.0011,-0.012,1.6e-06,-3.4e-07,1.4e-08,0,0,-3.4e-05,0,0,0,0,0,0,0,0,0.015,0.015,0.001,7.9,7.9,4.2,0.67,0.67,0.32,0.01,0.01,0.0024,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -690000,1,-0.0096,-0.012,8.6e-05,0.0063,0.0052,-0.06,0.0005,0.00055,-0.013,5.5e-06,-3.2e-06,1.2e-07,0,0,-6.3e-05,0,0,0,0,0,0,0,0,0.016,0.016,0.00062,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.0097,-0.013,9.9e-05,0.0086,0.0073,-0.063,0.0012,0.0012,-0.014,5.4e-06,-3.1e-06,1.1e-07,0,0,-9.5e-05,0,0,0,0,0,0,0,0,0.018,0.018,0.00079,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.0098,-0.013,0.00012,0.01,0.006,-0.077,0.00096,0.00072,-0.021,1.6e-05,-1.5e-05,3.3e-07,0,0,-9.5e-05,0,0,0,0,0,0,0,0,0.019,0.019,0.00053,1.3,1.3,2,0.2,0.2,0.43,0.0099,0.01,0.00068,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -990000,1,-0.0099,-0.013,0.00013,0.015,0.0064,-0.092,0.0022,0.0014,-0.029,1.6e-05,-1.5e-05,3.3e-07,0,0,-9.5e-05,0,0,0,0,0,0,0,0,0.021,0.021,0.00065,1.5,1.5,2,0.3,0.3,0.61,0.0099,0.01,0.00068,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -1090000,1,-0.01,-0.014,0.00012,0.016,0.0051,-0.11,0.0018,0.00086,-0.039,4.1e-05,-6.2e-05,9.3e-07,0,0,-9.5e-05,0,0,0,0,0,0,0,0,0.023,0.023,0.00046,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,9.8e-05,0.02,0.0053,-0.12,0.0035,0.0014,-0.051,4.1e-05,-6.2e-05,9.3e-07,0,0,-9.5e-05,0,0,0,0,0,0,0,0,0.025,0.025,0.00054,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.00015,0.02,0.0044,-0.14,0.0027,0.00089,-0.064,8.5e-05,-0.00019,2.3e-06,0,0,-9.6e-05,0,0,0,0,0,0,0,0,0.026,0.026,0.00041,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.00016,0.026,0.0042,-0.15,0.0051,0.0013,-0.078,8.5e-05,-0.00019,2.3e-06,0,0,-9.6e-05,0,0,0,0,0,0,0,0,0.028,0.028,0.00047,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.00014,0.024,0.003,-0.16,0.0038,0.00084,-0.093,0.00015,-0.00046,4.6e-06,0,0,-9.8e-05,0,0,0,0,0,0,0,0,0.027,0.027,0.00036,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.01,-0.014,0.00013,0.031,0.0035,-0.18,0.0065,0.0012,-0.11,0.00015,-0.00046,4.6e-06,0,0,-9.8e-05,0,0,0,0,0,0,0,0,0.03,0.03,0.00042,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,9.6e-05,0.028,-8.3e-05,-0.19,0.0045,0.00063,-0.13,0.00021,-0.00088,7.8e-06,0,0,-0.0001,0,0,0,0,0,0,0,0,0.025,0.025,0.00033,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,6.5e-05,0.035,-0.0019,-0.2,0.0076,0.00055,-0.15,0.00021,-0.00088,7.8e-06,0,0,-0.0001,0,0,0,0,0,0,0,0,0.028,0.028,0.00037,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,4.4e-05,0.043,-0.0031,-0.22,0.011,0.0003,-0.17,0.00021,-0.00088,7.8e-06,0,0,-0.0001,0,0,0,0,0,0,0,0,0.031,0.031,0.00042,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,3.6e-05,0.036,-0.0047,-0.23,0.0082,-0.00027,-0.19,0.00022,-0.0014,1.2e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,0.024,0.024,0.00033,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,-4.6e-06,0.041,-0.0072,-0.24,0.012,-0.00085,-0.22,0.00022,-0.0014,1.2e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,0.026,0.026,0.00037,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.3e-05,0.033,-0.0068,-0.26,0.0079,-0.00096,-0.24,0.00017,-0.002,1.7e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,0.02,0.02,0.0003,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,-3e-05,0.039,-0.0093,-0.27,0.011,-0.0017,-0.27,0.00017,-0.002,1.7e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,0.022,0.022,0.00033,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,-3.2e-05,0.03,-0.0087,-0.29,0.0074,-0.0015,-0.3,9.2e-05,-0.0025,2.1e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,0.016,0.016,0.00027,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,-5.3e-05,0.034,-0.011,-0.3,0.011,-0.0024,-0.32,9.2e-05,-0.0025,2.1e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,0.017,0.017,0.0003,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,-5.7e-05,0.026,-0.009,-0.31,0.0068,-0.0018,-0.36,-1.2e-05,-0.003,2.5e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,0.013,0.013,0.00025,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,-6.4e-05,0.03,-0.011,-0.33,0.0096,-0.0028,-0.39,-1.2e-05,-0.003,2.5e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,0.014,0.014,0.00027,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,-8.6e-05,0.023,-0.0093,-0.34,0.0061,-0.0019,-0.42,-0.00012,-0.0033,2.7e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,0.011,0.011,0.00023,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.00014,0.027,-0.011,-0.35,0.0086,-0.0029,-0.46,-0.00012,-0.0033,2.7e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,0.012,0.012,0.00025,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,-0.00011,0.022,-0.0095,-0.36,0.0056,-0.0021,-0.49,-0.00023,-0.0036,3e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,0.009,0.009,0.00021,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,-0.00011,0.024,-0.011,-0.38,0.0079,-0.0031,-0.53,-0.00023,-0.0036,3e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,0.01,0.0099,0.00023,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.00018,0.019,-0.0086,-0.39,0.0052,-0.0021,-0.57,-0.00034,-0.0039,3.2e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,0.0078,0.0078,0.0002,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.00014,0.023,-0.011,-0.4,0.0073,-0.0031,-0.61,-0.00034,-0.0039,3.2e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,0.0086,0.0086,0.00022,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.00018,0.018,-0.0091,-0.42,0.0049,-0.0021,-0.65,-0.00044,-0.0041,3.3e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,0.0069,0.0069,0.00019,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.0002,0.021,-0.012,-0.43,0.0068,-0.0031,-0.69,-0.00044,-0.0041,3.3e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,0.0076,0.0076,0.0002,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.00018,0.016,-0.011,-0.44,0.0046,-0.0023,-0.73,-0.00054,-0.0044,3.5e-05,0,0,-0.0001,0,0,0,0,0,0,0,0,0.0061,0.0061,0.00018,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,-6.8e-05,0.019,-0.014,-0.46,0.0064,-0.0035,-0.78,-0.00054,-0.0044,3.5e-05,0,0,-0.0001,0,0,0,0,0,0,0,0,0.0068,0.0067,0.00019,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,-3.3e-05,0.015,-0.013,-0.47,0.0043,-0.0026,-0.83,-0.00066,-0.0045,3.6e-05,0,0,-0.0001,0,0,0,0,0,0,0,0,0.0055,0.0055,0.00017,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,-7.2e-05,0.017,-0.014,-0.48,0.0059,-0.004,-0.87,-0.00066,-0.0045,3.6e-05,0,0,-0.0001,0,0,0,0,0,0,0,0,0.006,0.006,0.00018,0.54,0.54,2.4,0.17,0.17,24,0.0014,0.0014,2.1e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -3990000,1,-0.011,-0.012,-7.3e-05,0.02,-0.016,-0.5,0.0078,-0.0055,-0.92,-0.00066,-0.0045,3.6e-05,0,0,-0.0001,0,0,0,0,0,0,0,0,0.0066,0.0066,0.00019,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,-8.9e-05,0.017,-0.014,-0.51,0.0056,-0.0041,-0.97,-0.00078,-0.0047,3.8e-05,0,0,-9.6e-05,0,0,0,0,0,0,0,0,0.0054,0.0053,0.00017,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,-0.00012,0.02,-0.017,-0.53,0.0075,-0.0056,-1,-0.00078,-0.0047,3.8e-05,0,0,-9.6e-05,0,0,0,0,0,0,0,0,0.0058,0.0058,0.00018,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.00017,0.017,-0.012,-0.54,0.0054,-0.0041,-1.1,-0.00091,-0.0049,4e-05,0,0,-9.1e-05,0,0,0,0,0,0,0,0,0.0047,0.0047,0.00016,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.00016,0.018,-0.013,-0.55,0.0072,-0.0054,-1.1,-0.00091,-0.0049,4e-05,0,0,-9.1e-05,0,0,0,0,0,0,0,0,0.0052,0.0051,0.00017,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,-0.00011,0.015,-0.01,-0.57,0.0051,-0.0037,-1.2,-0.001,-0.005,4.2e-05,0,0,-8.7e-05,0,0,0,0,0,0,0,0,0.0042,0.0042,0.00015,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,-8.5e-05,0.018,-0.012,-0.58,0.0068,-0.0048,-1.2,-0.001,-0.005,4.2e-05,0,0,-8.7e-05,0,0,0,0,0,0,0,0,0.0045,0.0045,0.00016,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,-9.2e-05,0.014,-0.0099,-0.6,0.0049,-0.0034,-1.3,-0.0011,-0.0052,4.3e-05,0,0,-8.2e-05,0,0,0,0,0,0,0,0,0.0037,0.0036,0.00015,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,-0.00011,0.016,-0.011,-0.61,0.0064,-0.0044,-1.4,-0.0011,-0.0052,4.3e-05,0,0,-8.2e-05,0,0,0,0,0,0,0,0,0.004,0.0039,0.00015,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.00013,0.013,-0.01,-0.63,0.0046,-0.0032,-1.4,-0.0012,-0.0053,4.5e-05,0,0,-7.7e-05,0,0,0,0,0,0,0,0,0.0032,0.0032,0.00014,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.00016,0.015,-0.011,-0.64,0.006,-0.0043,-1.5,-0.0012,-0.0053,4.5e-05,0,0,-7.7e-05,0,0,0,0,0,0,0,0,0.0034,0.0034,0.00015,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,-0.00012,0.012,-0.0085,-0.66,0.0043,-0.0031,-1.6,-0.0013,-0.0054,4.6e-05,0,0,-7.3e-05,0,0,0,0,0,0,0,0,0.0028,0.0028,0.00013,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,-9.9e-05,0.013,-0.0099,-0.67,0.0056,-0.004,-1.6,-0.0013,-0.0054,4.6e-05,0,0,-7.3e-05,0,0,0,0,0,0,0,0,0.003,0.003,0.00014,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,-0.00012,0.0093,-0.0073,-0.68,0.0039,-0.0029,-1.7,-0.0013,-0.0055,4.7e-05,0,0,-6.9e-05,0,0,0,0,0,0,0,0,0.0024,0.0024,0.00013,0.3,0.3,2.9,0.12,0.12,51,0.00034,0.00034,9.2e-06,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -5390000,1,-0.0099,-0.011,-6.7e-05,0.0089,-0.0082,-0.7,0.0048,-0.0036,-1.8,-0.0013,-0.0055,4.7e-05,0,0,-6.9e-05,0,0,0,0,0,0,0,0,0.0026,0.0026,0.00014,0.36,0.36,2.9,0.16,0.16,53,0.00034,0.00034,9.2e-06,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -5490000,1,-0.0098,-0.011,-6.4e-05,0.0062,-0.0063,-0.71,0.0033,-0.0026,-1.8,-0.0014,-0.0055,4.7e-05,0,0,-6.6e-05,0,0,0,0,0,0,0,0,0.0021,0.0021,0.00012,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,-9e-05,0.0069,-0.0067,-0.73,0.0039,-0.0032,-1.9,-0.0014,-0.0055,4.7e-05,0,0,-6.6e-05,0,0,0,0,0,0,0,0,0.0022,0.0022,0.00013,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,-2.5e-05,0.0049,-0.004,-0.74,0.0027,-0.0022,-2,-0.0014,-0.0056,4.8e-05,0,0,-6.3e-05,0,0,0,0,0,0,0,0,0.0018,0.0018,0.00012,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.5e-05,0.0053,-0.003,-0.75,0.0031,-0.0026,-2,-0.0014,-0.0056,4.8e-05,0,0,-6.3e-05,0,0,0,0,0,0,0,0,0.0019,0.0019,0.00013,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,-6.5e-05,0.0045,-0.0012,0.0028,0.0022,-0.0017,-3.7e+02,-0.0014,-0.0056,4.9e-05,0,0,-6.4e-05,0,0,0,0,0,0,0,0,0.0015,0.0015,0.00012,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,-4.8e-05,0.0049,0.00023,0.015,0.0026,-0.0017,-3.7e+02,-0.0014,-0.0056,4.9e-05,0,0,-7.3e-05,0,0,0,0,0,0,0,0,0.0016,0.0016,0.00012,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,-6.9e-05,0.006,0.0013,-0.011,0.0032,-0.0016,-3.7e+02,-0.0014,-0.0056,4.9e-05,0,0,-6.4e-05,0,0,0,0,0,0,0,0,0.0017,0.0017,0.00013,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.78,-0.014,-0.0027,-0.63,0.0055,0.003,-0.005,0.0024,-0.00067,-3.7e+02,-0.0015,-0.0057,4.9e-05,0,0,-8.8e-05,-0.017,-0.0037,0.57,0,0,0,0,0,0.0013,0.0013,0.018,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.78,-0.014,-0.0027,-0.63,0.0057,0.0017,-0.012,0.0029,-0.00046,-3.7e+02,-0.0015,-0.0057,4.9e-05,0,0,-9.5e-05,-0.017,-0.0037,0.57,0,0,0,0,0,0.0014,0.0014,0.018,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.78,-0.014,-0.0028,-0.63,0.0068,0.0013,-0.05,0.0035,-0.00031,-3.7e+02,-0.0015,-0.0057,4.9e-05,0,0,-3.6e-05,-0.017,-0.0037,0.57,0,0,0,0,0,0.0014,0.0014,0.018,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.78,-0.014,-0.0028,-0.63,0.0071,0.00057,-0.052,0.0042,-0.00023,-3.7e+02,-0.0015,-0.0057,4.9e-05,0,0,-8.6e-05,-0.017,-0.0037,0.57,0,0,0,0,0,0.0014,0.0014,0.018,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.78,-0.014,-0.0028,-0.63,0.0075,0.00045,-0.099,0.0049,-0.00019,-3.7e+02,-0.0015,-0.0057,4.9e-05,0,0,9.5e-05,-0.017,-0.0037,0.57,0,0,0,0,0,0.0014,0.0014,0.018,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.78,-0.014,-0.0029,-0.63,0.0073,-0.00077,-0.076,0.0057,-0.00021,-3.7e+02,-0.0015,-0.0057,4.9e-05,0,0,-0.00022,-0.017,-0.0037,0.57,0,0,0,0,0,0.0014,0.0014,0.018,0.27,0.27,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.78,-0.014,-0.0029,-0.63,0.0084,-0.00058,-0.11,0.0064,-0.0003,-3.7e+02,-0.0015,-0.0057,4.9e-05,0,0,8.3e-06,-0.017,-0.0037,0.57,0,0,0,0,0,0.0014,0.0014,0.018,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.78,-0.014,-0.0029,-0.63,0.01,-0.00085,-0.12,0.0074,-0.00038,-3.7e+02,-0.0015,-0.0057,4.9e-05,0,0,-4.3e-05,-0.017,-0.0037,0.57,0,0,0,0,0,0.0014,0.0014,0.018,0.31,0.31,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.78,-0.014,-0.0029,-0.63,0.011,-0.0019,-0.12,0.0084,-0.00054,-3.7e+02,-0.0015,-0.0057,4.9e-05,0,0,-0.00034,-0.017,-0.0037,0.57,0,0,0,0,0,0.0015,0.0015,0.018,0.33,0.33,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.78,-0.014,-0.0029,-0.63,0.011,-0.0016,-0.13,0.0095,-0.00069,-3.7e+02,-0.0015,-0.0057,4.9e-05,0,0,-0.0007,-0.017,-0.0037,0.57,0,0,0,0,0,0.0015,0.0015,0.018,0.36,0.36,0.29,0.59,0.59,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.78,-0.014,-0.0029,-0.63,0.012,-0.0016,-0.15,0.011,-0.00086,-3.7e+02,-0.0015,-0.0057,4.9e-05,0,0,-0.00049,-0.017,-0.0037,0.57,0,0,0,0,0,0.0015,0.0015,0.018,0.39,0.39,0.24,0.67,0.67,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.78,-0.014,-0.0029,-0.63,0.014,-0.0018,-0.15,0.012,-0.001,-3.7e+02,-0.0015,-0.0057,4.9e-05,0,0,-0.0012,-0.017,-0.0037,0.57,0,0,0,0,0,0.0016,0.0015,0.018,0.42,0.42,0.2,0.76,0.77,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.78,-0.014,-0.003,-0.63,0.014,-0.0035,-0.16,0.013,-0.0013,-3.7e+02,-0.0015,-0.0057,4.9e-05,0,0,-0.0013,-0.017,-0.0037,0.57,0,0,0,0,0,0.0016,0.0016,0.018,0.45,0.46,0.18,0.86,0.86,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.78,-0.014,-0.003,-0.63,0.016,-0.0038,-0.16,0.015,-0.0016,-3.7e+02,-0.0015,-0.0057,4.9e-05,0,0,-0.0021,-0.017,-0.0037,0.57,0,0,0,0,0,0.0016,0.0016,0.018,0.5,0.5,0.15,0.98,0.98,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.78,-0.014,-0.003,-0.63,0.018,-0.005,-0.17,0.016,-0.0021,-3.7e+02,-0.0015,-0.0057,4.9e-05,0,0,-0.003,-0.017,-0.0037,0.57,0,0,0,0,0,0.0017,0.0017,0.018,0.54,0.54,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.78,-0.014,-0.0031,-0.63,0.019,-0.0056,-0.16,0.018,-0.0025,-3.7e+02,-0.0015,-0.0057,4.9e-05,0,0,-0.005,-0.017,-0.0037,0.57,0,0,0,0,0,0.0017,0.0017,0.018,0.58,0.59,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.78,-0.014,-0.0031,-0.63,0.021,-0.0065,-0.16,0.02,-0.003,-3.7e+02,-0.0015,-0.0057,4.9e-05,0,0,-0.007,-0.017,-0.0037,0.57,0,0,0,0,0,0.0017,0.0017,0.018,0.63,0.63,0.12,1.4,1.4,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.78,-0.014,-0.0031,-0.63,0.024,-0.0081,-0.16,0.022,-0.0037,-3.7e+02,-0.0015,-0.0057,4.9e-05,0,0,-0.0096,-0.017,-0.0037,0.57,0,0,0,0,0,0.0018,0.0018,0.018,0.69,0.69,0.11,1.6,1.6,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.78,-0.014,-0.0032,-0.63,0.025,-0.0091,-0.16,0.024,-0.0044,-3.7e+02,-0.0015,-0.0057,4.9e-05,0,0,-0.011,-0.017,-0.0037,0.57,0,0,0,0,0,0.0018,0.0018,0.018,0.74,0.74,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.78,-0.014,-0.0032,-0.63,0.027,-0.011,-0.17,0.027,-0.0054,-3.7e+02,-0.0015,-0.0057,4.9e-05,0,0,-0.011,-0.017,-0.0037,0.57,0,0,0,0,0,0.0019,0.0019,0.018,0.8,0.8,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.78,-0.014,-0.0032,-0.63,0.03,-0.012,-0.18,0.03,-0.0065,-3.7e+02,-0.0015,-0.0057,4.9e-05,0,0,-0.013,-0.017,-0.0037,0.57,0,0,0,0,0,0.0019,0.0019,0.018,0.87,0.87,0.099,2.2,2.2,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.78,-0.014,-0.0032,-0.63,0.031,-0.012,-0.17,0.033,-0.0074,-3.7e+02,-0.0015,-0.0057,4.8e-05,0,0,-0.017,-0.017,-0.0037,0.57,0,0,0,0,0,0.002,0.002,0.018,0.92,0.92,0.097,2.4,2.4,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.78,-0.014,-0.0033,-0.63,0.033,-0.014,-0.17,0.036,-0.0086,-3.7e+02,-0.0015,-0.0057,4.8e-05,0,0,-0.021,-0.017,-0.0037,0.57,0,0,0,0,0,0.002,0.002,0.018,1,1,0.097,2.6,2.6,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.78,-0.014,-0.0033,-0.63,0.035,-0.015,-0.17,0.039,-0.0096,-3.7e+02,-0.0015,-0.0057,4.8e-05,0,0,-0.025,-0.017,-0.0037,0.57,0,0,0,0,0,0.0021,0.0021,0.018,1.1,1.1,0.096,2.9,2.9,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.78,-0.014,-0.0033,-0.63,0.038,-0.017,-0.17,0.043,-0.011,-3.7e+02,-0.0015,-0.0057,4.8e-05,0,0,-0.029,-0.017,-0.0037,0.57,0,0,0,0,0,0.0021,0.0021,0.018,1.1,1.1,0.095,3.2,3.2,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.78,-0.014,-0.0033,-0.63,0.041,-0.018,-0.16,0.046,-0.012,-3.7e+02,-0.0015,-0.0057,4.8e-05,0,0,-0.035,-0.017,-0.0037,0.57,0,0,0,0,0,0.0022,0.0022,0.018,1.2,1.2,0.096,3.4,3.4,0.088,0.00013,0.00013,6.3e-06,0.04,0.04,0.033,0,0,0,0,0,0,0,0 -8790000,0.78,-0.014,-0.0033,-0.63,0.043,-0.019,-0.15,0.05,-0.013,-3.7e+02,-0.0015,-0.0057,4.8e-05,0,0,-0.041,-0.017,-0.0037,0.57,0,0,0,0,0,0.0022,0.0022,0.018,1.3,1.3,0.095,3.8,3.8,0.087,0.00013,0.00013,6.3e-06,0.04,0.04,0.032,0,0,0,0,0,0,0,0 -8890000,0.78,-0.014,-0.0033,-0.63,0.045,-0.02,-0.15,0.053,-0.015,-3.7e+02,-0.0015,-0.0057,4.8e-05,0,0,-0.045,-0.017,-0.0037,0.57,0,0,0,0,0,0.0023,0.0023,0.018,1.3,1.3,0.095,4.1,4.1,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.78,-0.014,-0.0034,-0.63,0.047,-0.02,-0.14,0.058,-0.016,-3.7e+02,-0.0015,-0.0057,4.8e-05,0,0,-0.051,-0.017,-0.0037,0.57,0,0,0,0,0,0.0023,0.0023,0.018,1.4,1.4,0.096,4.5,4.5,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.78,-0.014,-0.0034,-0.63,0.048,-0.02,-0.14,0.06,-0.017,-3.7e+02,-0.0015,-0.0057,4.8e-05,0,0,-0.053,-0.017,-0.0037,0.57,0,0,0,0,0,0.0023,0.0023,0.018,1.5,1.5,0.095,4.8,4.8,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.78,-0.014,-0.0034,-0.63,0.05,-0.021,-0.14,0.065,-0.019,-3.7e+02,-0.0015,-0.0057,4.8e-05,0,0,-0.057,-0.017,-0.0037,0.57,0,0,0,0,0,0.0024,0.0024,0.018,1.6,1.6,0.094,5.3,5.3,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.78,-0.014,-0.0034,-0.63,0.05,-0.022,-0.14,0.067,-0.02,-3.7e+02,-0.0015,-0.0057,4.8e-05,0,0,-0.061,-0.017,-0.0037,0.57,0,0,0,0,0,0.0024,0.0024,0.018,1.6,1.6,0.093,5.5,5.6,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.78,-0.014,-0.0033,-0.63,0.051,-0.024,-0.14,0.073,-0.022,-3.7e+02,-0.0015,-0.0057,4.8e-05,0,0,-0.065,-0.017,-0.0037,0.57,0,0,0,0,0,0.0025,0.0025,0.018,1.8,1.8,0.093,6.1,6.1,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.78,-0.014,-0.0033,-0.63,0.052,-0.023,-0.13,0.074,-0.022,-3.7e+02,-0.0015,-0.0057,4.8e-05,0,0,-0.068,-0.017,-0.0037,0.57,0,0,0,0,0,0.0025,0.0025,0.018,1.8,1.8,0.091,6.3,6.3,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.78,-0.014,-0.0033,-0.63,0.054,-0.025,-0.13,0.079,-0.024,-3.7e+02,-0.0015,-0.0057,4.8e-05,0,0,-0.072,-0.017,-0.0037,0.57,0,0,0,0,0,0.0026,0.0025,0.018,1.9,1.9,0.09,7,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.78,-0.014,-0.0034,-0.63,0.054,-0.025,-0.12,0.08,-0.025,-3.7e+02,-0.0015,-0.0057,4.8e-05,0,0,-0.077,-0.017,-0.0037,0.57,0,0,0,0,0,0.0025,0.0025,0.018,1.9,1.9,0.089,7.1,7.1,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.78,-0.014,-0.0034,-0.63,0.055,-0.027,-0.11,0.086,-0.027,-3.7e+02,-0.0015,-0.0057,4.8e-05,0,0,-0.082,-0.017,-0.0037,0.57,0,0,0,0,0,0.0026,0.0026,0.018,2,2,0.087,7.9,7.9,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.78,-0.014,-0.0033,-0.63,0.054,-0.027,-0.11,0.084,-0.027,-3.7e+02,-0.0015,-0.0057,4.7e-05,0,0,-0.085,-0.017,-0.0037,0.57,0,0,0,0,0,0.0025,0.0025,0.018,2,2,0.084,7.9,7.9,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.78,-0.014,-0.0034,-0.63,0.056,-0.028,-0.1,0.09,-0.029,-3.7e+02,-0.0015,-0.0057,4.7e-05,0,0,-0.089,-0.017,-0.0037,0.57,0,0,0,0,0,0.0026,0.0026,0.018,2.1,2.1,0.083,8.7,8.7,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.78,-0.014,-0.0034,-0.63,0.054,-0.026,-0.096,0.088,-0.029,-3.7e+02,-0.0014,-0.0058,4.7e-05,0,0,-0.091,-0.017,-0.0037,0.57,0,0,0,0,0,0.0026,0.0026,0.018,2.1,2.1,0.08,8.7,8.7,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.78,-0.014,-0.0034,-0.63,0.056,-0.028,-0.096,0.094,-0.032,-3.7e+02,-0.0014,-0.0058,4.7e-05,0,0,-0.093,-0.017,-0.0037,0.57,0,0,0,0,0,0.0026,0.0026,0.018,2.2,2.2,0.078,9.5,9.5,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.78,-0.014,-0.0034,-0.63,0.057,-0.029,-0.084,0.1,-0.034,-3.7e+02,-0.0014,-0.0058,4.7e-05,0,0,-0.098,-0.017,-0.0037,0.57,0,0,0,0,0,0.0027,0.0027,0.018,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.78,-0.014,-0.0033,-0.63,0.01,-0.021,0.0096,0.00093,-0.0019,-3.7e+02,-0.0014,-0.0058,4.7e-05,-6.6e-08,5e-08,-0.1,-0.017,-0.0037,0.57,0,0,0,0,0,0.0028,0.0028,0.018,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.78,-0.014,-0.0034,-0.63,0.012,-0.023,0.023,0.0021,-0.004,-3.7e+02,-0.0014,-0.0058,4.7e-05,-1.7e-06,1.3e-06,-0.1,-0.017,-0.0037,0.57,0,0,0,0,0,0.0029,0.0029,0.018,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.78,-0.014,-0.0033,-0.63,0.012,-0.012,0.026,0.002,-0.00088,-3.7e+02,-0.0014,-0.0058,4.7e-05,0.00035,-4.5e-05,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.0029,0.0029,0.018,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.78,-0.014,-0.0034,-0.63,0.014,-0.012,0.03,0.0032,-0.002,-3.7e+02,-0.0014,-0.0058,4.7e-05,0.00035,-4.4e-05,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.003,0.003,0.018,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.78,-0.013,-0.0032,-0.63,0.014,-0.0063,0.024,0.003,-0.00081,-3.7e+02,-0.0014,-0.0058,4.6e-05,0.00072,-6e-07,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.0029,0.0029,0.018,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.78,-0.013,-0.0032,-0.63,0.015,-0.0065,0.02,0.0044,-0.0015,-3.7e+02,-0.0014,-0.0058,4.6e-05,0.00072,-1.2e-06,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.003,0.003,0.018,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.78,-0.013,-0.0031,-0.63,0.02,-0.00025,0.014,0.0068,0.00019,-3.7e+02,-0.0015,-0.0059,4.6e-05,0.0013,-0.00032,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.0028,0.0028,0.018,0.1,0.1,0.12,0.1,0.1,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.78,-0.013,-0.0031,-0.63,0.022,-0.00052,0.019,0.0088,0.00019,-3.7e+02,-0.0015,-0.0059,4.6e-05,0.0013,-0.00031,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.0029,0.0029,0.018,0.13,0.13,0.11,0.11,0.11,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.78,-0.013,-0.003,-0.63,0.022,0.0037,0.0076,0.011,0.0019,-3.7e+02,-0.0015,-0.006,4.5e-05,0.0018,-0.00034,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.0025,0.0025,0.018,0.11,0.11,0.084,0.11,0.11,0.069,9.2e-05,9.2e-05,6.3e-06,0.038,0.038,0.01,0,0,0,0,0,0,0,0 -11290000,0.78,-0.013,-0.003,-0.63,0.023,0.004,0.0073,0.013,0.0023,-3.7e+02,-0.0015,-0.006,4.5e-05,0.0018,-0.00034,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.0026,0.0026,0.018,0.13,0.13,0.078,0.12,0.12,0.072,9.2e-05,9.2e-05,6.3e-06,0.038,0.038,0.01,0,0,0,0,0,0,0,0 -11390000,0.78,-0.013,-0.0028,-0.63,0.014,0.0025,0.0017,0.0069,0.0012,-3.7e+02,-0.0013,-0.006,4.2e-05,0.0018,0.00065,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.0022,0.0022,0.018,0.1,0.1,0.063,0.084,0.084,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.78,-0.013,-0.0028,-0.63,0.017,0.0023,0.0025,0.0084,0.0015,-3.7e+02,-0.0013,-0.006,4.2e-05,0.0018,0.00065,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.0022,0.0022,0.018,0.13,0.13,0.058,0.092,0.092,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.78,-0.014,-0.0025,-0.63,0.012,0.0011,-0.0034,0.0051,0.0009,-3.7e+02,-0.0012,-0.0059,3.9e-05,0.0018,0.0013,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.0018,0.0018,0.018,0.1,0.1,0.049,0.07,0.07,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.78,-0.014,-0.0025,-0.63,0.014,0.00019,-0.0079,0.0064,0.00099,-3.7e+02,-0.0012,-0.0059,3.9e-05,0.0018,0.0013,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.0019,0.0019,0.018,0.12,0.12,0.046,0.078,0.078,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.78,-0.014,-0.0021,-0.63,0.0066,0.0049,-0.0098,0.0025,0.0025,-3.7e+02,-0.0011,-0.006,3.6e-05,0.0023,0.002,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.0016,0.0016,0.018,0.097,0.097,0.039,0.062,0.062,0.063,6.4e-05,6.4e-05,6.3e-06,0.037,0.037,0.01,0,0,0,0,0,0,0,0 -11890000,0.78,-0.014,-0.0021,-0.63,0.0059,0.0057,-0.011,0.0031,0.003,-3.7e+02,-0.0011,-0.006,3.6e-05,0.0023,0.002,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.0016,0.0016,0.018,0.12,0.12,0.037,0.07,0.07,0.063,6.4e-05,6.4e-05,6.3e-06,0.037,0.037,0.01,0,0,0,0,0,0,0,0 -11990000,0.78,-0.014,-0.002,-0.63,-3.8e-05,0.0075,-0.016,0.00039,0.0036,-3.7e+02,-0.0011,-0.006,3.4e-05,0.0025,0.0023,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.0014,0.0014,0.018,0.092,0.092,0.033,0.057,0.057,0.061,5.8e-05,5.8e-05,6.3e-06,0.037,0.037,0.01,0,0,0,0,0,0,0,0 -12090000,0.78,-0.014,-0.0019,-0.63,-0.0011,0.0072,-0.022,0.00033,0.0044,-3.7e+02,-0.0011,-0.006,3.4e-05,0.0025,0.0023,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.0014,0.0014,0.018,0.11,0.11,0.031,0.066,0.066,0.061,5.8e-05,5.8e-05,6.3e-06,0.037,0.037,0.01,0,0,0,0,0,0,0,0 -12190000,0.78,-0.014,-0.0021,-0.63,0.002,0.0037,-0.017,0.002,0.003,-3.7e+02,-0.0011,-0.006,3.5e-05,0.0022,0.0022,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.0012,0.0012,0.018,0.086,0.086,0.028,0.054,0.054,0.059,5.3e-05,5.3e-05,6.3e-06,0.037,0.037,0.01,0,0,0,0,0,0,0,0 -12290000,0.78,-0.014,-0.0021,-0.63,0.003,0.0038,-0.016,0.0022,0.0034,-3.7e+02,-0.0011,-0.006,3.5e-05,0.0022,0.0022,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.0012,0.0012,0.018,0.1,0.1,0.028,0.063,0.063,0.059,5.3e-05,5.3e-05,6.3e-06,0.037,0.037,0.01,0,0,0,0,0,0,0,0 -12390000,0.78,-0.014,-0.0021,-0.63,0.0043,0.002,-0.015,0.0032,0.0024,-3.7e+02,-0.0011,-0.006,3.6e-05,0.0021,0.0022,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.0011,0.0011,0.018,0.08,0.08,0.026,0.052,0.052,0.057,4.9e-05,4.9e-05,6.3e-06,0.036,0.036,0.01,0,0,0,0,0,0,0,0 -12490000,0.78,-0.014,-0.0021,-0.63,0.0046,0.0011,-0.018,0.0036,0.0026,-3.7e+02,-0.0011,-0.006,3.6e-05,0.002,0.0022,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.0011,0.0011,0.018,0.094,0.094,0.026,0.061,0.061,0.057,4.9e-05,4.9e-05,6.3e-06,0.036,0.036,0.01,0,0,0,0,0,0,0,0 -12590000,0.78,-0.014,-0.0018,-0.63,-0.0065,-0.00066,-0.023,-0.0024,0.002,-3.7e+02,-0.001,-0.006,3.3e-05,0.0021,0.0025,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.001,0.001,0.018,0.075,0.075,0.025,0.051,0.051,0.055,4.6e-05,4.6e-05,6.3e-06,0.036,0.036,0.0099,0,0,0,0,0,0,0,0 -12690000,0.78,-0.014,-0.0019,-0.63,-0.0082,-0.0016,-0.027,-0.0031,0.0019,-3.7e+02,-0.001,-0.006,3.3e-05,0.0021,0.0025,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.001,0.001,0.018,0.087,0.087,0.025,0.059,0.059,0.055,4.6e-05,4.6e-05,6.3e-06,0.036,0.036,0.0098,0,0,0,0,0,0,0,0 -12790000,0.78,-0.014,-0.0017,-0.63,-0.015,-0.0012,-0.03,-0.007,0.0016,-3.7e+02,-0.00095,-0.006,3.2e-05,0.0021,0.0026,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00094,0.00094,0.018,0.07,0.07,0.024,0.05,0.05,0.053,4.3e-05,4.3e-05,6.3e-06,0.036,0.036,0.0097,0,0,0,0,0,0,0,0 -12890000,0.78,-0.014,-0.0017,-0.63,-0.017,-0.00087,-0.029,-0.0086,0.0015,-3.7e+02,-0.00095,-0.006,3.2e-05,0.002,0.0027,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00097,0.00097,0.018,0.08,0.08,0.025,0.059,0.059,0.054,4.3e-05,4.3e-05,6.3e-06,0.036,0.036,0.0096,0,0,0,0,0,0,0,0 -12990000,0.78,-0.014,-0.0019,-0.63,-0.0096,-0.00054,-0.03,-0.0043,0.0016,-3.7e+02,-0.0011,-0.006,3.4e-05,0.002,0.0027,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.0009,0.0009,0.018,0.069,0.069,0.025,0.06,0.06,0.052,4.1e-05,4.1e-05,6.3e-06,0.036,0.036,0.0094,0,0,0,0,0,0,0,0 -13090000,0.78,-0.014,-0.0019,-0.63,-0.011,-0.00052,-0.03,-0.0053,0.0015,-3.7e+02,-0.0011,-0.006,3.4e-05,0.002,0.0027,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00093,0.00093,0.018,0.079,0.079,0.025,0.069,0.069,0.052,4.1e-05,4.1e-05,6.3e-06,0.036,0.036,0.0094,0,0,0,0,0,0,0,0 -13190000,0.78,-0.014,-0.0021,-0.63,-0.0046,-0.0016,-0.027,-0.00068,0.0014,-3.7e+02,-0.0011,-0.006,3.6e-05,0.0018,0.0029,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00087,0.00087,0.018,0.068,0.068,0.025,0.07,0.07,0.051,3.9e-05,3.9e-05,6.3e-06,0.036,0.036,0.0091,0,0,0,0,0,0,0,0 -13290000,0.78,-0.014,-0.0021,-0.63,-0.005,-0.0023,-0.024,-0.0012,0.0012,-3.7e+02,-0.0011,-0.006,3.6e-05,0.0017,0.003,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.0009,0.0009,0.018,0.077,0.077,0.027,0.08,0.08,0.051,3.9e-05,3.9e-05,6.3e-06,0.036,0.036,0.0091,0,0,0,0,0,0,0,0 -13390000,0.78,-0.014,-0.0021,-0.63,-0.0022,-0.0026,-0.02,0.0013,0.0014,-3.7e+02,-0.0012,-0.006,3.7e-05,0.0016,0.0032,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00085,0.00085,0.018,0.067,0.067,0.026,0.079,0.079,0.05,3.7e-05,3.7e-05,6.3e-06,0.036,0.036,0.0088,0,0,0,0,0,0,0,0 -13490000,0.78,-0.014,-0.0021,-0.63,-0.0022,-0.0024,-0.019,0.0011,0.0012,-3.7e+02,-0.0012,-0.006,3.7e-05,0.0015,0.0033,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00087,0.00087,0.018,0.075,0.075,0.028,0.091,0.091,0.05,3.7e-05,3.7e-05,6.3e-06,0.036,0.036,0.0087,0,0,0,0,0,0,0,0 -13590000,0.78,-0.014,-0.0022,-0.63,0.0008,-0.0035,-0.021,0.0035,0.0014,-3.7e+02,-0.0012,-0.006,3.8e-05,0.0015,0.0033,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00083,0.00083,0.018,0.065,0.065,0.028,0.089,0.089,0.05,3.5e-05,3.5e-05,6.3e-06,0.036,0.036,0.0084,0,0,0,0,0,0,0,0 -13690000,0.78,-0.014,-0.0021,-0.63,0.00024,-0.0054,-0.025,0.0036,0.001,-3.7e+02,-0.0012,-0.006,3.8e-05,0.0016,0.0033,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00086,0.00085,0.018,0.073,0.073,0.029,0.1,0.1,0.05,3.5e-05,3.5e-05,6.3e-06,0.036,0.036,0.0083,0,0,0,0,0,0,0,0 -13790000,0.78,-0.014,-0.0022,-0.63,0.0027,-0.0063,-0.027,0.0049,-0.00084,-3.7e+02,-0.0012,-0.006,3.9e-05,0.0016,0.0034,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00081,0.00081,0.018,0.055,0.055,0.029,0.074,0.074,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.78,-0.014,-0.0022,-0.63,0.0022,-0.0067,-0.031,0.0051,-0.0015,-3.7e+02,-0.0012,-0.006,3.9e-05,0.0017,0.0034,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00083,0.00083,0.018,0.062,0.062,0.03,0.084,0.084,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.78,-0.014,-0.0023,-0.63,0.0041,-0.0075,-0.03,0.0055,-0.0024,-3.7e+02,-0.0012,-0.006,3.9e-05,0.0017,0.0036,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.0008,0.0008,0.018,0.05,0.05,0.03,0.065,0.065,0.05,3.2e-05,3.2e-05,6.3e-06,0.036,0.036,0.0074,0,0,0,0,0,0,0,0 -14090000,0.78,-0.014,-0.0023,-0.63,0.0043,-0.0083,-0.031,0.006,-0.0031,-3.7e+02,-0.0012,-0.006,3.9e-05,0.0017,0.0036,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00082,0.00082,0.018,0.056,0.056,0.031,0.073,0.073,0.05,3.2e-05,3.2e-05,6.3e-06,0.036,0.036,0.0073,0,0,0,0,0,0,0,0 -14190000,0.78,-0.013,-0.0023,-0.63,0.0062,-0.0064,-0.033,0.0077,-0.0019,-3.7e+02,-0.0013,-0.006,4e-05,0.0016,0.0039,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.0008,0.00079,0.018,0.047,0.047,0.031,0.059,0.059,0.05,3e-05,3e-05,6.3e-06,0.036,0.036,0.0069,0,0,0,0,0,0,0,0 -14290000,0.78,-0.013,-0.0023,-0.63,0.0062,-0.0078,-0.032,0.0083,-0.0026,-3.7e+02,-0.0013,-0.006,4e-05,0.0016,0.0039,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00082,0.00082,0.018,0.053,0.053,0.032,0.066,0.066,0.051,3e-05,3e-05,6.3e-06,0.036,0.036,0.0067,0,0,0,0,0,0,0,0 -14390000,0.78,-0.013,-0.0023,-0.63,0.0084,-0.0075,-0.034,0.0094,-0.0017,-3.7e+02,-0.0013,-0.006,4.1e-05,0.0015,0.0043,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00079,0.00079,0.018,0.045,0.045,0.031,0.055,0.055,0.05,2.9e-05,2.9e-05,6.3e-06,0.036,0.036,0.0063,0,0,0,0,0,0,0,0 -14490000,0.78,-0.013,-0.0023,-0.63,0.0095,-0.0091,-0.037,0.01,-0.0025,-3.7e+02,-0.0013,-0.006,4.1e-05,0.0016,0.0042,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00081,0.00081,0.018,0.051,0.051,0.032,0.062,0.062,0.051,2.9e-05,2.9e-05,6.3e-06,0.036,0.036,0.0062,0,0,0,0,0,0,0,0 -14590000,0.78,-0.013,-0.0021,-0.63,0.005,-0.01,-0.037,0.0064,-0.0031,-3.7e+02,-0.0012,-0.006,3.9e-05,0.0014,0.0038,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00079,0.00078,0.018,0.044,0.044,0.031,0.052,0.052,0.051,2.7e-05,2.7e-05,6.3e-06,0.036,0.036,0.0058,0,0,0,0,0,0,0,0 -14690000,0.78,-0.013,-0.0021,-0.63,0.0035,-0.0083,-0.034,0.0068,-0.0041,-3.7e+02,-0.0012,-0.006,3.9e-05,0.0013,0.0039,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00081,0.00081,0.018,0.05,0.05,0.032,0.059,0.059,0.051,2.7e-05,2.7e-05,6.3e-06,0.036,0.036,0.0056,0,0,0,0,0,0,0,0 -14790000,0.78,-0.013,-0.002,-0.63,0.00042,-0.0086,-0.03,0.004,-0.0044,-3.7e+02,-0.0012,-0.006,3.8e-05,0.0011,0.0037,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00078,0.00078,0.018,0.043,0.043,0.031,0.05,0.05,0.051,2.6e-05,2.6e-05,6.3e-06,0.036,0.036,0.0053,0,0,0,0,0,0,0,0 -14890000,0.78,-0.013,-0.002,-0.63,-0.00075,-0.011,-0.033,0.004,-0.0053,-3.7e+02,-0.0012,-0.006,3.8e-05,0.0011,0.0037,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.0008,0.0008,0.018,0.049,0.049,0.031,0.057,0.057,0.052,2.6e-05,2.6e-05,6.3e-06,0.036,0.036,0.0051,0,0,0,0,0,0,0,0 -14990000,0.78,-0.013,-0.0019,-0.63,-0.0011,-0.0081,-0.029,0.0033,-0.004,-3.7e+02,-0.0012,-0.006,3.8e-05,0.00074,0.0039,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00077,0.00077,0.018,0.043,0.043,0.03,0.049,0.049,0.051,2.4e-05,2.4e-05,6.3e-06,0.035,0.036,0.0048,0,0,0,0,0,0,0,0 -15090000,0.78,-0.013,-0.0019,-0.63,-0.0014,-0.0086,-0.032,0.0032,-0.0048,-3.7e+02,-0.0012,-0.006,3.8e-05,0.00079,0.0038,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00079,0.00079,0.019,0.048,0.048,0.031,0.055,0.055,0.052,2.4e-05,2.4e-05,6.3e-06,0.035,0.036,0.0046,0,0,0,0,0,0,0,0 -15190000,0.78,-0.013,-0.0018,-0.63,-0.0017,-0.0079,-0.029,0.0027,-0.0037,-3.7e+02,-0.0012,-0.006,3.8e-05,0.00048,0.004,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00076,0.00076,0.019,0.042,0.042,0.03,0.048,0.048,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.78,-0.013,-0.0018,-0.63,-0.0022,-0.0087,-0.027,0.0025,-0.0045,-3.7e+02,-0.0012,-0.006,3.8e-05,0.0004,0.0041,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00078,0.00078,0.019,0.047,0.047,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.78,-0.013,-0.0017,-0.63,-0.0032,-0.0081,-0.024,0.0025,-0.0042,-3.7e+02,-0.0012,-0.0061,3.8e-05,0.00013,0.0042,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00075,0.00075,0.019,0.045,0.045,0.029,0.057,0.057,0.051,2.1e-05,2.1e-05,6.3e-06,0.035,0.035,0.0039,0,0,0,0,0,0,0,0 -15490000,0.78,-0.013,-0.0018,-0.63,-0.0045,-0.0084,-0.024,0.0021,-0.005,-3.7e+02,-0.0012,-0.0061,3.8e-05,0.00014,0.0042,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00077,0.00077,0.019,0.05,0.05,0.029,0.064,0.064,0.053,2.1e-05,2.1e-05,6.3e-06,0.035,0.035,0.0037,0,0,0,0,0,0,0,0 -15590000,0.78,-0.013,-0.0017,-0.63,-0.0039,-0.0072,-0.023,0.0023,-0.0046,-3.7e+02,-0.0012,-0.0061,3.9e-05,-7.5e-05,0.0045,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00074,0.00074,0.019,0.047,0.047,0.028,0.066,0.066,0.052,2e-05,2e-05,6.3e-06,0.035,0.035,0.0035,0,0,0,0,0,0,0,0 -15690000,0.78,-0.013,-0.0017,-0.63,-0.0042,-0.0075,-0.023,0.002,-0.0053,-3.7e+02,-0.0012,-0.0061,3.9e-05,-7.6e-05,0.0045,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00076,0.00076,0.019,0.052,0.052,0.028,0.075,0.075,0.052,2e-05,2e-05,6.3e-06,0.035,0.035,0.0033,0,0,0,0,0,0,0,0 -15790000,0.78,-0.013,-0.0018,-0.63,-0.0046,-0.0075,-0.026,0.0018,-0.0053,-3.7e+02,-0.0013,-0.0061,3.9e-05,4e-05,0.0045,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00072,0.00072,0.019,0.043,0.043,0.027,0.06,0.06,0.051,1.8e-05,1.8e-05,6.3e-06,0.034,0.034,0.0031,0,0,0,0,0,0,0,0 -15890000,0.78,-0.013,-0.0017,-0.63,-0.0056,-0.0078,-0.024,0.0013,-0.0061,-3.7e+02,-0.0013,-0.0061,3.9e-05,1.8e-05,0.0046,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00074,0.00074,0.019,0.048,0.048,0.027,0.068,0.068,0.052,1.8e-05,1.8e-05,6.3e-06,0.034,0.034,0.003,0,0,0,0,0,0,0,0 -15990000,0.78,-0.013,-0.0017,-0.63,-0.0053,-0.0079,-0.019,0.0014,-0.0059,-3.7e+02,-0.0013,-0.0061,3.9e-05,5.4e-06,0.0048,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00071,0.0007,0.019,0.041,0.041,0.026,0.056,0.056,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.78,-0.013,-0.0017,-0.63,-0.007,-0.0087,-0.016,0.00075,-0.0067,-3.7e+02,-0.0013,-0.0061,3.9e-05,-6.2e-05,0.0048,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00072,0.00072,0.019,0.046,0.046,0.025,0.063,0.063,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.78,-0.013,-0.0017,-0.63,-0.0068,-0.007,-0.014,0.00096,-0.0049,-3.7e+02,-0.0013,-0.0061,3.9e-05,-0.00033,0.0052,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00069,0.00069,0.019,0.039,0.039,0.025,0.053,0.053,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.78,-0.013,-0.0017,-0.63,-0.0085,-0.007,-0.016,0.00019,-0.0057,-3.7e+02,-0.0013,-0.0061,3.9e-05,-0.00032,0.0052,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.0007,0.0007,0.019,0.044,0.044,0.024,0.06,0.06,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.78,-0.013,-0.0017,-0.63,-0.0074,-0.0046,-0.015,0.00053,-0.0043,-3.7e+02,-0.0013,-0.0061,4e-05,-0.0005,0.0055,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00067,0.00067,0.019,0.038,0.038,0.023,0.051,0.051,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.78,-0.013,-0.0017,-0.63,-0.0065,-0.0053,-0.018,-0.00014,-0.0048,-3.7e+02,-0.0013,-0.0061,4e-05,-0.00046,0.0055,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00069,0.00069,0.019,0.043,0.043,0.023,0.057,0.057,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.78,-0.013,-0.0016,-0.63,-0.0091,-0.00012,-0.018,-0.0026,-0.00096,-3.7e+02,-0.0013,-0.0062,3.9e-05,-0.0014,0.0053,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00066,0.00066,0.019,0.038,0.038,0.022,0.049,0.049,0.051,1.3e-05,1.3e-05,6.3e-06,0.033,0.033,0.002,0,0,0,0,0,0,0,0 -16690000,0.78,-0.013,-0.0016,-0.63,-0.0095,-0.00064,-0.015,-0.0036,-0.001,-3.7e+02,-0.0013,-0.0062,3.9e-05,-0.0014,0.0054,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00067,0.00067,0.019,0.042,0.042,0.022,0.056,0.056,0.051,1.3e-05,1.3e-05,6.3e-06,0.033,0.033,0.0019,0,0,0,0,0,0,0,0 -16790000,0.78,-0.013,-0.0014,-0.63,-0.011,0.0035,-0.014,-0.0053,0.0019,-3.7e+02,-0.0013,-0.0062,3.8e-05,-0.0021,0.0053,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00064,0.00064,0.019,0.037,0.037,0.021,0.048,0.048,0.05,1.2e-05,1.2e-05,6.3e-06,0.033,0.033,0.0018,0,0,0,0,0,0,0,0 -16890000,0.78,-0.013,-0.0014,-0.63,-0.011,0.003,-0.011,-0.0064,0.0023,-3.7e+02,-0.0013,-0.0062,3.8e-05,-0.0022,0.0053,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00065,0.00065,0.019,0.041,0.041,0.021,0.054,0.054,0.051,1.2e-05,1.2e-05,6.3e-06,0.033,0.033,0.0017,0,0,0,0,0,0,0,0 -16990000,0.78,-0.013,-0.0015,-0.63,-0.012,0.00085,-0.01,-0.0063,0.00041,-3.7e+02,-0.0013,-0.0062,3.9e-05,-0.0018,0.0053,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00063,0.00063,0.019,0.036,0.036,0.02,0.047,0.047,0.05,1.1e-05,1.1e-05,6.3e-06,0.033,0.033,0.0016,0,0,0,0,0,0,0,0 -17090000,0.78,-0.013,-0.0014,-0.63,-0.013,-0.00019,-0.01,-0.0075,0.00047,-3.7e+02,-0.0013,-0.0062,3.9e-05,-0.0017,0.0053,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00064,0.00064,0.019,0.041,0.041,0.02,0.053,0.053,0.05,1.1e-05,1.1e-05,6.3e-06,0.033,0.033,0.0016,0,0,0,0,0,0,0,0 -17190000,0.78,-0.013,-0.0015,-0.63,-0.013,-0.0035,-0.011,-0.0072,-0.001,-3.7e+02,-0.0013,-0.0062,3.9e-05,-0.0014,0.0053,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00061,0.00061,0.019,0.036,0.036,0.019,0.047,0.047,0.049,9.7e-06,9.7e-06,6.3e-06,0.032,0.032,0.0015,0,0,0,0,0,0,0,0 -17290000,0.78,-0.013,-0.0015,-0.63,-0.015,-0.0051,-0.0066,-0.0086,-0.0014,-3.7e+02,-0.0013,-0.0062,3.9e-05,-0.0015,0.0054,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00062,0.00062,0.019,0.04,0.04,0.019,0.053,0.053,0.049,9.7e-06,9.7e-06,6.3e-06,0.032,0.032,0.0014,0,0,0,0,0,0,0,0 -17390000,0.78,-0.013,-0.0016,-0.63,-0.013,-0.0067,-0.0047,-0.0066,-0.0024,-3.7e+02,-0.0013,-0.0062,4e-05,-0.0012,0.0058,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00059,0.00059,0.019,0.035,0.035,0.018,0.046,0.046,0.048,8.8e-06,8.8e-06,6.3e-06,0.032,0.032,0.0013,0,0,0,0,0,0,0,0 -17490000,0.78,-0.013,-0.0016,-0.63,-0.014,-0.0066,-0.003,-0.008,-0.0031,-3.7e+02,-0.0013,-0.0062,4e-05,-0.0012,0.0059,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.0006,0.0006,0.019,0.039,0.039,0.018,0.052,0.052,0.049,8.8e-06,8.8e-06,6.3e-06,0.032,0.032,0.0013,0,0,0,0,0,0,0,0 -17590000,0.78,-0.013,-0.0016,-0.63,-0.013,-0.0075,0.0026,-0.0062,-0.0036,-3.7e+02,-0.0013,-0.0061,4e-05,-0.001,0.0063,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00058,0.00058,0.019,0.035,0.035,0.017,0.046,0.046,0.048,8e-06,8e-06,6.3e-06,0.032,0.032,0.0012,0,0,0,0,0,0,0,0 -17690000,0.78,-0.013,-0.0016,-0.63,-0.014,-0.0087,0.0019,-0.0076,-0.0044,-3.7e+02,-0.0013,-0.0061,4e-05,-0.001,0.0063,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00059,0.00059,0.019,0.039,0.039,0.017,0.052,0.052,0.048,8e-06,8e-06,6.3e-06,0.032,0.032,0.0012,0,0,0,0,0,0,0,0 -17790000,0.78,-0.012,-0.0016,-0.63,-0.011,-0.0076,0.00061,-0.0047,-0.0042,-3.7e+02,-0.0014,-0.0061,4.2e-05,-0.00099,0.0072,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00057,0.00057,0.019,0.037,0.037,0.016,0.054,0.054,0.048,7.4e-06,7.4e-06,6.3e-06,0.032,0.032,0.0011,0,0,0,0,0,0,0,0 -17890000,0.78,-0.012,-0.0016,-0.63,-0.012,-0.0074,0.00071,-0.0058,-0.005,-3.7e+02,-0.0014,-0.0061,4.2e-05,-0.00099,0.0072,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00058,0.00058,0.019,0.041,0.041,0.016,0.062,0.062,0.048,7.4e-06,7.4e-06,6.3e-06,0.032,0.032,0.0011,0,0,0,0,0,0,0,0 -17990000,0.78,-0.012,-0.0017,-0.63,-0.0091,-0.0049,0.0019,-0.0023,-0.0046,-3.7e+02,-0.0014,-0.0061,4.3e-05,-0.00093,0.0081,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00056,0.00056,0.019,0.039,0.039,0.016,0.063,0.063,0.047,6.8e-06,6.8e-06,6.3e-06,0.032,0.032,0.001,0,0,0,0,0,0,0,0 -18090000,0.78,-0.012,-0.0017,-0.63,-0.0095,-0.005,0.0043,-0.0033,-0.005,-3.7e+02,-0.0014,-0.0061,4.3e-05,-0.00094,0.0082,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00057,0.00057,0.019,0.043,0.043,0.016,0.072,0.072,0.047,6.8e-06,6.8e-06,6.3e-06,0.032,0.032,0.00097,0,0,0,0,0,0,0,0 -18190000,0.78,-0.012,-0.0017,-0.63,-0.0066,-0.0046,0.0056,-0.0003,-0.0037,-3.7e+02,-0.0015,-0.0061,4.4e-05,-0.00096,0.0087,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00054,0.00054,0.019,0.035,0.035,0.015,0.058,0.058,0.047,6e-06,6e-06,6.3e-06,0.031,0.031,0.00092,0,0,0,0,0,0,0,0 -18290000,0.78,-0.012,-0.0016,-0.63,-0.0063,-0.0041,0.0068,-0.00094,-0.0041,-3.7e+02,-0.0015,-0.0061,4.4e-05,-0.00097,0.0087,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00054,0.00054,0.019,0.039,0.039,0.015,0.065,0.065,0.046,6e-06,6e-06,6.3e-06,0.031,0.031,0.00089,0,0,0,0,0,0,0,0 -18390000,0.78,-0.012,-0.0017,-0.63,-0.005,-0.0048,0.008,0.001,-0.0032,-3.7e+02,-0.0015,-0.0062,4.4e-05,-0.00099,0.0092,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00052,0.00052,0.019,0.033,0.033,0.014,0.054,0.054,0.046,5.4e-06,5.4e-06,6.3e-06,0.031,0.031,0.00084,0,0,0,0,0,0,0,0 -18490000,0.77,-0.012,-0.0017,-0.63,-0.0052,-0.0054,0.0076,0.00049,-0.0037,-3.7e+02,-0.0015,-0.0062,4.4e-05,-0.001,0.0092,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00053,0.00053,0.019,0.037,0.037,0.014,0.061,0.061,0.046,5.4e-06,5.4e-06,6.3e-06,0.031,0.031,0.00082,0,0,0,0,0,0,0,0 -18590000,0.77,-0.012,-0.0017,-0.63,-0.0052,-0.0048,0.0058,0.00056,-0.0027,-3.7e+02,-0.0015,-0.0062,4.4e-05,-0.0011,0.0093,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00051,0.0005,0.019,0.032,0.032,0.014,0.052,0.052,0.045,4.9e-06,4.9e-06,6.3e-06,0.031,0.031,0.00078,0,0,0,0,0,0,0,0 -18690000,0.77,-0.012,-0.0016,-0.63,-0.0052,-0.0042,0.0039,5.3e-05,-0.0032,-3.7e+02,-0.0015,-0.0062,4.4e-05,-0.0011,0.0093,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00051,0.00051,0.019,0.035,0.035,0.013,0.058,0.058,0.045,4.9e-06,4.9e-06,6.3e-06,0.031,0.031,0.00075,0,0,0,0,0,0,0,0 -18790000,0.77,-0.012,-0.0016,-0.63,-0.0046,-0.0037,0.0035,0.00021,-0.0025,-3.7e+02,-0.0015,-0.0062,4.5e-05,-0.0012,0.0095,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00049,0.00049,0.019,0.031,0.031,0.013,0.05,0.05,0.045,4.5e-06,4.5e-06,6.3e-06,0.031,0.031,0.00072,0,0,0,0,0,0,0,0 -18890000,0.77,-0.012,-0.0017,-0.63,-0.0046,-0.0043,0.0042,-0.00024,-0.003,-3.7e+02,-0.0015,-0.0062,4.5e-05,-0.0012,0.0095,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.0005,0.0005,0.019,0.034,0.034,0.013,0.056,0.056,0.045,4.5e-06,4.5e-06,6.3e-06,0.031,0.031,0.0007,0,0,0,0,0,0,0,0 -18990000,0.78,-0.012,-0.0017,-0.63,-0.0016,-0.0045,0.0028,0.0025,-0.0023,-3.7e+02,-0.0015,-0.0062,4.5e-05,-0.0012,0.01,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00048,0.00048,0.019,0.03,0.03,0.012,0.048,0.048,0.044,4.1e-06,4.1e-06,6.3e-06,0.031,0.031,0.00066,0,0,0,0,0,0,0,0 -19090000,0.78,-0.012,-0.0017,-0.63,-0.0015,-0.0052,0.0058,0.0023,-0.0028,-3.7e+02,-0.0015,-0.0062,4.5e-05,-0.0012,0.01,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00049,0.00049,0.019,0.033,0.033,0.012,0.054,0.054,0.044,4.1e-06,4.1e-06,6.3e-06,0.031,0.031,0.00065,0,0,0,0,0,0,0,0 -19190000,0.78,-0.012,-0.0018,-0.63,0.0015,-0.0045,0.0059,0.0045,-0.0022,-3.7e+02,-0.0016,-0.0062,4.6e-05,-0.0012,0.01,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00047,0.00047,0.019,0.029,0.029,0.012,0.047,0.047,0.044,3.7e-06,3.7e-06,6.3e-06,0.03,0.03,0.00062,0,0,0,0,0,0,0,0 -19290000,0.78,-0.012,-0.0018,-0.63,0.0017,-0.0038,0.0086,0.0047,-0.0026,-3.7e+02,-0.0016,-0.0062,4.6e-05,-0.0012,0.01,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00048,0.00048,0.019,0.032,0.032,0.012,0.053,0.053,0.044,3.7e-06,3.7e-06,6.3e-06,0.03,0.03,0.0006,0,0,0,0,0,0,0,0 -19390000,0.78,-0.012,-0.0017,-0.63,0.0015,-0.0023,0.012,0.0038,-0.0021,-3.7e+02,-0.0016,-0.0062,4.6e-05,-0.0013,0.01,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00046,0.00046,0.019,0.028,0.028,0.012,0.046,0.046,0.043,3.4e-06,3.4e-06,6.3e-06,0.03,0.03,0.00058,0,0,0,0,0,0,0,0 -19490000,0.78,-0.012,-0.0018,-0.63,0.003,-0.0014,0.0088,0.0041,-0.0023,-3.7e+02,-0.0016,-0.0062,4.6e-05,-0.0013,0.01,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00047,0.00047,0.019,0.031,0.031,0.011,0.052,0.052,0.043,3.4e-06,3.4e-06,6.3e-06,0.03,0.03,0.00056,0,0,0,0,0,0,0,0 -19590000,0.78,-0.012,-0.0018,-0.63,0.0026,3.2e-05,0.0081,0.0033,-0.0018,-3.7e+02,-0.0016,-0.0062,4.5e-05,-0.0014,0.01,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00045,0.00045,0.019,0.027,0.027,0.011,0.046,0.046,0.042,3.1e-06,3.1e-06,6.3e-06,0.03,0.03,0.00054,0,0,0,0,0,0,0,0 -19690000,0.78,-0.012,-0.0018,-0.63,0.0024,0.0022,0.0096,0.0036,-0.0017,-3.7e+02,-0.0016,-0.0062,4.5e-05,-0.0014,0.01,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00046,0.00046,0.019,0.03,0.03,0.011,0.051,0.051,0.042,3.1e-06,3.1e-06,6.3e-06,0.03,0.03,0.00052,0,0,0,0,0,0,0,0 -19790000,0.78,-0.012,-0.0018,-0.63,0.0026,0.0035,0.01,0.0029,-0.0014,-3.7e+02,-0.0015,-0.0062,4.5e-05,-0.0014,0.01,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00045,0.00044,0.019,0.027,0.027,0.011,0.045,0.045,0.042,2.9e-06,2.9e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -19890000,0.78,-0.012,-0.0017,-0.63,0.0043,0.004,0.011,0.0033,-0.00097,-3.7e+02,-0.0015,-0.0062,4.5e-05,-0.0014,0.01,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00045,0.00045,0.019,0.029,0.029,0.011,0.05,0.05,0.042,2.9e-06,2.9e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -19990000,0.78,-0.012,-0.0017,-0.63,0.0049,0.0052,0.014,0.0027,-0.00079,-3.7e+02,-0.0015,-0.0062,4.5e-05,-0.0013,0.01,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00044,0.00044,0.019,0.026,0.026,0.01,0.045,0.045,0.041,2.6e-06,2.6e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -20090000,0.78,-0.012,-0.0017,-0.63,0.0052,0.0072,0.014,0.0032,-0.00019,-3.7e+02,-0.0015,-0.0062,4.5e-05,-0.0013,0.01,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00044,0.00044,0.019,0.029,0.029,0.01,0.05,0.05,0.042,2.6e-06,2.6e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -20190000,0.78,-0.012,-0.0017,-0.63,0.0037,0.0081,0.017,0.0014,-0.00021,-3.7e+02,-0.0015,-0.0062,4.5e-05,-0.0013,0.0098,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00043,0.00043,0.019,0.026,0.026,0.01,0.044,0.044,0.041,2.4e-06,2.4e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -20290000,0.78,-0.012,-0.0017,-0.63,0.0048,0.0099,0.015,0.0019,0.00068,-3.7e+02,-0.0015,-0.0062,4.5e-05,-0.0013,0.0098,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00043,0.00043,0.019,0.028,0.028,0.0099,0.05,0.05,0.041,2.4e-06,2.4e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -20390000,0.78,-0.012,-0.0016,-0.63,0.0041,0.011,0.017,0.0004,0.00048,-3.7e+02,-0.0015,-0.0062,4.5e-05,-0.0012,0.0095,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00042,0.00042,0.019,0.025,0.025,0.0097,0.044,0.044,0.041,2.2e-06,2.2e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -20490000,0.78,-0.012,-0.0017,-0.63,0.0046,0.012,0.016,0.00085,0.0016,-3.7e+02,-0.0015,-0.0062,4.5e-05,-0.0012,0.0095,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00043,0.00042,0.019,0.027,0.027,0.0096,0.049,0.049,0.041,2.2e-06,2.2e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -20590000,0.78,-0.012,-0.0017,-0.63,0.0038,0.011,0.013,0.00064,0.0012,-3.7e+02,-0.0015,-0.0062,4.4e-05,-0.0011,0.0094,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00042,0.00041,0.019,0.025,0.025,0.0094,0.044,0.044,0.04,2.1e-06,2.1e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -20690000,0.78,-0.012,-0.0017,-0.63,0.0037,0.013,0.015,0.001,0.0024,-3.7e+02,-0.0015,-0.0062,4.4e-05,-0.0011,0.0094,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00042,0.00042,0.019,0.027,0.027,0.0093,0.049,0.049,0.04,2.1e-06,2.1e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -20790000,0.78,-0.012,-0.0017,-0.63,0.0045,0.012,0.015,0.00084,0.0019,-3.7e+02,-0.0015,-0.0062,4.4e-05,-0.00089,0.0093,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00041,0.00041,0.019,0.024,0.024,0.0091,0.043,0.043,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 -20890000,0.77,-0.012,-0.0017,-0.63,0.0046,0.015,0.014,0.0013,0.0032,-3.7e+02,-0.0015,-0.0062,4.4e-05,-0.00089,0.0093,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00041,0.00041,0.019,0.026,0.026,0.0091,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 -20990000,0.78,-0.012,-0.0017,-0.63,0.0053,0.016,0.015,0.0013,0.0032,-3.7e+02,-0.0015,-0.0061,4.4e-05,-0.00072,0.0092,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00041,0.00041,0.019,0.026,0.026,0.0089,0.051,0.051,0.039,1.8e-06,1.8e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -21090000,0.77,-0.012,-0.0017,-0.63,0.0052,0.018,0.015,0.0018,0.0049,-3.7e+02,-0.0015,-0.0061,4.4e-05,-0.00072,0.0092,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00041,0.00041,0.019,0.028,0.028,0.0089,0.057,0.057,0.039,1.8e-06,1.8e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -21190000,0.77,-0.012,-0.0018,-0.63,0.0051,0.017,0.014,0.0017,0.0045,-3.7e+02,-0.0015,-0.0061,4.4e-05,-0.00051,0.0091,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00041,0.0004,0.019,0.027,0.027,0.0087,0.06,0.06,0.039,1.7e-06,1.7e-06,6.3e-06,0.029,0.03,0.0005,0,0,0,0,0,0,0,0 -21290000,0.77,-0.012,-0.0018,-0.63,0.0054,0.02,0.016,0.0022,0.0063,-3.7e+02,-0.0015,-0.0061,4.4e-05,-0.00051,0.0091,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00041,0.00041,0.019,0.029,0.029,0.0086,0.066,0.066,0.039,1.7e-06,1.7e-06,6.3e-06,0.029,0.03,0.0005,0,0,0,0,0,0,0,0 -21390000,0.77,-0.012,-0.0018,-0.63,0.0052,0.02,0.016,0.0015,0.0056,-3.7e+02,-0.0015,-0.0061,4.4e-05,-0.00035,0.0089,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.0004,0.0004,0.019,0.025,0.025,0.0085,0.055,0.055,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 -21490000,0.77,-0.012,-0.0018,-0.63,0.0057,0.021,0.015,0.0021,0.0077,-3.7e+02,-0.0015,-0.0061,4.4e-05,-0.00036,0.0089,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.0004,0.0004,0.019,0.027,0.027,0.0085,0.061,0.061,0.038,1.5e-06,1.5e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -21590000,0.77,-0.012,-0.0018,-0.63,0.0059,0.02,0.015,0.0015,0.0069,-3.7e+02,-0.0015,-0.0061,4.4e-05,-0.00019,0.0088,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00039,0.00039,0.019,0.023,0.023,0.0083,0.052,0.052,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 -21690000,0.77,-0.012,-0.0018,-0.63,0.0057,0.021,0.017,0.0021,0.009,-3.7e+02,-0.0015,-0.0061,4.4e-05,-0.0002,0.0088,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00039,0.00039,0.02,0.025,0.025,0.0084,0.058,0.058,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 -21790000,0.77,-0.012,-0.0017,-0.63,0.0044,0.021,0.015,0.00046,0.01,-3.7e+02,-0.0015,-0.0061,4.4e-05,-0.00029,0.0086,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00039,0.00038,0.02,0.022,0.022,0.0082,0.049,0.049,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 -21890000,0.77,-0.012,-0.0017,-0.63,0.0044,0.021,0.016,0.00091,0.013,-3.7e+02,-0.0015,-0.0061,4.4e-05,-0.0003,0.0086,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00039,0.00039,0.02,0.024,0.024,0.0082,0.055,0.055,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.77,-0.012,-0.0017,-0.63,0.0031,0.023,0.016,-0.00047,0.013,-3.7e+02,-0.0015,-0.0061,4.4e-05,-0.00035,0.0085,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00038,0.00038,0.02,0.021,0.021,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 -22090000,0.77,-0.012,-0.0017,-0.63,0.0035,0.022,0.015,-0.00015,0.016,-3.7e+02,-0.0015,-0.0061,4.4e-05,-0.00036,0.0085,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00038,0.00038,0.02,0.023,0.023,0.0081,0.053,0.053,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.77,-0.012,-0.0017,-0.63,0.0038,0.019,0.015,-0.00015,0.013,-3.7e+02,-0.0015,-0.0061,4.4e-05,-5.3e-05,0.0084,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00038,0.00038,0.02,0.021,0.021,0.008,0.046,0.046,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.77,-0.012,-0.0017,-0.63,0.005,0.021,0.015,0.00028,0.015,-3.7e+02,-0.0015,-0.0061,4.4e-05,-6.1e-05,0.0084,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00038,0.00038,0.02,0.022,0.022,0.008,0.051,0.051,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.77,-0.012,-0.0017,-0.63,0.0059,0.018,0.017,0.00025,0.012,-3.7e+02,-0.0015,-0.0061,4.4e-05,0.00021,0.0083,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00037,0.00037,0.02,0.02,0.02,0.0079,0.045,0.045,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 -22490000,0.77,-0.012,-0.0017,-0.63,0.0066,0.019,0.018,0.00087,0.014,-3.7e+02,-0.0015,-0.0061,4.4e-05,0.00022,0.0082,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00038,0.00037,0.02,0.022,0.022,0.0079,0.05,0.05,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 -22590000,0.77,-0.012,-0.0017,-0.63,0.0073,0.02,0.017,0.0019,0.015,-3.7e+02,-0.0015,-0.0061,4.4e-05,0.0003,0.0083,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00037,0.00037,0.02,0.021,0.021,0.0078,0.052,0.052,0.036,1e-06,1e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -22690000,0.77,-0.012,-0.0018,-0.63,0.0086,0.02,0.018,0.0027,0.017,-3.7e+02,-0.0015,-0.0061,4.4e-05,0.00029,0.0083,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00038,0.00037,0.02,0.023,0.023,0.0079,0.058,0.058,0.037,1e-06,1e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -22790000,0.77,-0.012,-0.0017,-0.63,0.0088,0.018,0.019,0.0026,0.017,-3.7e+02,-0.0015,-0.0061,4.4e-05,0.00046,0.0081,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00037,0.00037,0.02,0.023,0.023,0.0078,0.06,0.06,0.036,9.9e-07,9.9e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -22890000,0.77,-0.012,-0.0018,-0.63,0.01,0.018,0.021,0.0035,0.019,-3.7e+02,-0.0015,-0.0061,4.4e-05,0.00046,0.0081,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00037,0.00037,0.02,0.025,0.024,0.0078,0.066,0.066,0.036,1e-06,1e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -22990000,0.77,-0.012,-0.0017,-0.63,0.0097,0.018,0.022,0.0033,0.018,-3.7e+02,-0.0015,-0.0061,4.4e-05,0.0006,0.008,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00037,0.00037,0.02,0.024,0.024,0.0078,0.069,0.069,0.036,9.5e-07,9.5e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -23090000,0.77,-0.012,-0.0017,-0.63,0.01,0.018,0.022,0.0043,0.02,-3.7e+02,-0.0015,-0.0061,4.4e-05,0.00061,0.008,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00037,0.00037,0.02,0.026,0.026,0.0078,0.076,0.076,0.036,9.5e-07,9.5e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -23190000,0.77,-0.012,-0.0017,-0.63,0.0062,0.018,0.024,-0.00044,0.019,-3.7e+02,-0.0014,-0.0061,4.3e-05,0.00068,0.0076,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00037,0.00037,0.02,0.025,0.025,0.0077,0.078,0.078,0.036,9.1e-07,9.1e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -23290000,0.77,-0.012,-0.0016,-0.63,0.0065,0.02,0.024,0.00019,0.021,-3.7e+02,-0.0014,-0.0061,4.3e-05,0.00067,0.0076,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00037,0.00037,0.02,0.027,0.027,0.0078,0.085,0.085,0.036,9.1e-07,9.1e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -23390000,0.77,-0.012,-0.0017,-0.63,0.0025,0.019,0.022,-0.0049,0.02,-3.7e+02,-0.0014,-0.0061,4.3e-05,0.00075,0.0072,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00037,0.00037,0.02,0.026,0.026,0.0077,0.087,0.087,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 -23490000,0.77,-0.0092,-0.0038,-0.63,0.0089,0.022,-0.012,-0.0044,0.022,-3.7e+02,-0.0014,-0.0061,4.3e-05,0.00074,0.0072,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00037,0.00037,0.02,0.028,0.028,0.0078,0.095,0.095,0.036,8.8e-07,8.7e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -23590000,0.78,-0.00091,-0.0082,-0.63,0.019,0.021,-0.044,-0.0051,0.016,-3.7e+02,-0.0014,-0.0061,4.3e-05,0.00097,0.007,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00036,0.00036,0.02,0.022,0.022,0.0077,0.072,0.072,0.035,8e-07,8e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -23690000,0.78,0.0048,-0.0073,-0.63,0.048,0.036,-0.094,-0.0018,0.019,-3.7e+02,-0.0014,-0.0061,4.3e-05,0.00097,0.007,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00036,0.00036,0.02,0.024,0.024,0.0078,0.079,0.079,0.036,8e-07,8e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -23790000,0.78,0.0011,-0.0047,-0.63,0.069,0.051,-0.15,-0.0037,0.015,-3.7e+02,-0.0014,-0.006,4.2e-05,0.0012,0.0063,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00035,0.02,0.02,0.02,0.0077,0.062,0.062,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 -23890000,0.78,-0.0052,-0.0027,-0.63,0.083,0.063,-0.2,0.004,0.021,-3.7e+02,-0.0014,-0.006,4.2e-05,0.0012,0.0063,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00036,0.00035,0.02,0.022,0.022,0.0078,0.068,0.068,0.035,7.5e-07,7.4e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -23990000,0.78,-0.01,-0.0017,-0.63,0.073,0.061,-0.25,-0.0071,0.018,-3.7e+02,-0.0013,-0.006,4e-05,0.0016,0.0046,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00036,0.00035,0.02,0.021,0.022,0.0077,0.071,0.071,0.035,7.2e-07,7.2e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -24090000,0.78,-0.0087,-0.0028,-0.63,0.074,0.062,-0.3,0.00025,0.024,-3.7e+02,-0.0013,-0.006,4e-05,0.0016,0.0046,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00036,0.00035,0.02,0.023,0.023,0.0078,0.077,0.077,0.035,7.2e-07,7.2e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -24190000,0.78,-0.0066,-0.0035,-0.63,0.067,0.059,-0.35,-0.012,0.019,-3.7e+02,-0.0013,-0.006,3.8e-05,0.0021,0.0028,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00035,0.02,0.023,0.023,0.0077,0.079,0.079,0.035,7e-07,7e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -24290000,0.78,-0.0057,-0.004,-0.63,0.076,0.065,-0.41,-0.0053,0.026,-3.7e+02,-0.0013,-0.006,3.8e-05,0.0021,0.0028,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00035,0.02,0.024,0.024,0.0078,0.086,0.086,0.036,7e-07,7e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -24390000,0.78,-0.0063,-0.004,-0.63,0.068,0.059,-0.46,-0.024,0.014,-3.7e+02,-0.0012,-0.006,3.7e-05,0.003,0.00035,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00035,0.02,0.024,0.024,0.0078,0.088,0.088,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 -24490000,0.78,-0.0021,-0.0044,-0.63,0.079,0.067,-0.51,-0.017,0.02,-3.7e+02,-0.0012,-0.006,3.7e-05,0.003,0.00035,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00035,0.02,0.025,0.026,0.0078,0.096,0.096,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 -24590000,0.78,0.0011,-0.0044,-0.63,0.078,0.067,-0.56,-0.038,0.0078,-3.7e+02,-0.0012,-0.006,3.6e-05,0.0041,-0.0023,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00035,0.02,0.025,0.025,0.0078,0.098,0.098,0.035,6.5e-07,6.5e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -24690000,0.78,0.0021,-0.0044,-0.63,0.098,0.085,-0.64,-0.029,0.015,-3.7e+02,-0.0012,-0.006,3.6e-05,0.0041,-0.0023,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00035,0.02,0.027,0.027,0.0078,0.11,0.11,0.035,6.5e-07,6.5e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -24790000,0.78,0.00032,-0.0041,-0.63,0.093,0.09,-0.73,-0.059,0.0052,-3.7e+02,-0.0011,-0.0059,3.1e-05,0.0049,-0.0056,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00034,0.02,0.026,0.026,0.0078,0.11,0.11,0.035,6.3e-07,6.3e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -24890000,0.78,0.0022,-0.0056,-0.63,0.11,0.1,-0.75,-0.049,0.015,-3.7e+02,-0.0011,-0.0059,3.1e-05,0.0049,-0.0056,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00035,0.02,0.028,0.028,0.0078,0.12,0.12,0.035,6.3e-07,6.3e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -24990000,0.78,0.0036,-0.0071,-0.63,0.11,0.11,-0.81,-0.082,0.0019,-3.7e+02,-0.001,-0.0059,2.6e-05,0.006,-0.0093,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00035,0.02,0.027,0.027,0.0078,0.12,0.12,0.035,6.1e-07,6.1e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -25090000,0.78,0.003,-0.0074,-0.63,0.13,0.12,-0.86,-0.07,0.013,-3.7e+02,-0.001,-0.0059,2.6e-05,0.006,-0.0093,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00035,0.02,0.028,0.029,0.0079,0.13,0.13,0.035,6.1e-07,6.1e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -25190000,0.78,0.00067,-0.0068,-0.63,0.11,0.11,-0.91,-0.13,-0.018,-3.7e+02,-0.00087,-0.0059,2e-05,0.0081,-0.015,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00034,0.02,0.027,0.028,0.0078,0.13,0.13,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 -25290000,0.78,0.0076,-0.008,-0.63,0.14,0.12,-0.96,-0.12,-0.0063,-3.7e+02,-0.00087,-0.0059,2e-05,0.0081,-0.015,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00035,0.02,0.029,0.03,0.0079,0.14,0.14,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 -25390000,0.78,0.013,-0.0084,-0.63,0.13,0.12,-1,-0.18,-0.039,-3.7e+02,-0.00075,-0.0058,1.5e-05,0.01,-0.021,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00036,0.02,0.028,0.028,0.0078,0.14,0.14,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 -25490000,0.78,0.015,-0.0085,-0.63,0.17,0.15,-1.1,-0.17,-0.025,-3.7e+02,-0.00075,-0.0058,1.5e-05,0.01,-0.021,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00036,0.02,0.03,0.031,0.0079,0.15,0.15,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 -25590000,0.78,0.013,-0.0083,-0.63,0.2,0.18,-1.1,-0.15,-0.0084,-3.7e+02,-0.00075,-0.0058,1.5e-05,0.01,-0.021,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00036,0.02,0.032,0.033,0.008,0.16,0.16,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 -25690000,0.78,0.02,-0.011,-0.63,0.24,0.21,-1.2,-0.13,0.011,-3.7e+02,-0.00075,-0.0058,1.5e-05,0.01,-0.021,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00038,0.02,0.034,0.035,0.008,0.17,0.17,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 -25790000,0.78,0.026,-0.013,-0.63,0.3,0.24,-1.2,-0.099,0.033,-3.7e+02,-0.00075,-0.0058,1.5e-05,0.01,-0.021,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00041,0.02,0.036,0.038,0.0081,0.18,0.18,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 -25890000,0.78,0.027,-0.013,-0.63,0.35,0.28,-1.3,-0.066,0.06,-3.7e+02,-0.00075,-0.0058,1.5e-05,0.01,-0.021,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00042,0.02,0.039,0.042,0.0082,0.2,0.2,0.036,5.7e-07,5.7e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -25990000,0.78,0.024,-0.013,-0.63,0.41,0.32,-1.3,-0.028,0.09,-3.7e+02,-0.00075,-0.0058,1.5e-05,0.01,-0.021,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.0004,0.02,0.042,0.045,0.0082,0.21,0.21,0.036,5.7e-07,5.7e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -26090000,0.78,0.034,-0.017,-0.62,0.46,0.36,-1.4,0.015,0.12,-3.7e+02,-0.00075,-0.0058,1.5e-05,0.01,-0.021,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00046,0.02,0.044,0.049,0.0082,0.23,0.23,0.036,5.7e-07,5.7e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -26190000,0.78,0.044,-0.018,-0.62,0.53,0.41,-1.3,0.065,0.16,-3.7e+02,-0.00075,-0.0058,1.5e-05,0.01,-0.021,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00036,0.00052,0.02,0.047,0.053,0.0083,0.25,0.25,0.036,5.7e-07,5.7e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -26290000,0.78,0.046,-0.019,-0.62,0.61,0.46,-1.3,0.12,0.21,-3.7e+02,-0.00075,-0.0058,1.5e-05,0.01,-0.021,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00036,0.00054,0.02,0.051,0.057,0.0083,0.27,0.27,0.036,5.8e-07,5.8e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -26390000,0.78,0.043,-0.018,-0.62,0.69,0.52,-1.3,0.19,0.25,-3.7e+02,-0.00075,-0.0058,1.5e-05,0.01,-0.021,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00036,0.00052,0.02,0.054,0.062,0.0084,0.29,0.29,0.036,5.8e-07,5.8e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -26490000,0.78,0.059,-0.024,-0.62,0.77,0.57,-1.3,0.26,0.31,-3.7e+02,-0.00075,-0.0058,1.5e-05,0.01,-0.021,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00037,0.00066,0.02,0.058,0.068,0.0084,0.31,0.32,0.036,5.8e-07,5.8e-07,6.4e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -26590000,0.78,0.076,-0.029,-0.63,0.88,0.65,-1.3,0.34,0.37,-3.7e+02,-0.00075,-0.0058,1.5e-05,0.01,-0.021,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.0004,0.00084,0.02,0.063,0.075,0.0085,0.33,0.34,0.037,5.8e-07,5.8e-07,6.4e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -26690000,0.78,0.079,-0.03,-0.63,1,0.74,-1.3,0.44,0.44,-3.7e+02,-0.00075,-0.0058,1.5e-05,0.01,-0.021,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.0004,0.00088,0.02,0.068,0.083,0.0085,0.36,0.37,0.037,5.8e-07,5.8e-07,6.4e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -26790000,0.77,0.073,-0.029,-0.63,1.1,0.82,-1.3,0.54,0.52,-3.7e+02,-0.00075,-0.0058,1.5e-05,0.0099,-0.021,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00039,0.00081,0.02,0.074,0.093,0.0085,0.39,0.4,0.037,5.8e-07,5.8e-07,6.4e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -26890000,0.77,0.096,-0.035,-0.63,1.3,0.91,-1.3,0.66,0.6,-3.7e+02,-0.00075,-0.0058,1.5e-05,0.0099,-0.021,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00044,0.0011,0.019,0.08,0.1,0.0086,0.42,0.43,0.037,5.8e-07,5.8e-07,6.4e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -26990000,0.77,0.12,-0.04,-0.63,1.4,1,-1.3,0.8,0.7,-3.7e+02,-0.00075,-0.0058,1.5e-05,0.0099,-0.021,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.0005,0.0015,0.019,0.087,0.12,0.0086,0.45,0.47,0.037,5.8e-07,5.8e-07,6.4e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -27090000,0.77,0.12,-0.04,-0.63,1.6,1.1,-1.3,0.95,0.81,-3.7e+02,-0.00075,-0.0058,1.5e-05,0.0098,-0.021,-0.13,-0.1,-0.022,0.51,0.0047,-0.1,-0.04,0,0,0.00052,0.0015,0.019,0.096,0.13,0.0087,0.49,0.51,0.037,5.8e-07,5.8e-07,6.4e-06,0.029,0.029,0.0005,0.0013,0.00033,0.0013,0.00053,0.0013,0.0013,0,0 -27190000,0.77,0.11,-0.038,-0.63,1.8,1.3,-1.2,1.1,0.93,-3.7e+02,-0.00075,-0.0058,1.5e-05,0.0098,-0.021,-0.13,-0.11,-0.022,0.5,0.0048,-0.1,-0.041,0,0,0.00049,0.0013,0.019,0.11,0.15,0.0087,0.53,0.55,0.037,5.8e-07,5.8e-07,6.4e-06,0.029,0.029,0.0005,0.0012,0.00022,0.0013,0.00033,0.0013,0.0013,0,0 -27290000,0.77,0.096,-0.034,-0.63,1.9,1.4,-1.2,1.3,1.1,-3.7e+02,-0.00075,-0.0058,1.5e-05,0.0098,-0.021,-0.13,-0.11,-0.023,0.5,0.0051,-0.1,-0.042,0,0,0.00045,0.0011,0.02,0.11,0.16,0.0088,0.57,0.6,0.037,5.9e-07,5.9e-07,6.4e-06,0.029,0.029,0.0005,0.0012,0.00016,0.0013,0.00023,0.0013,0.0013,0,0 -27390000,0.77,0.08,-0.029,-0.63,2,1.4,-1.2,1.5,1.2,-3.7e+02,-0.00074,-0.0058,1.5e-05,0.0097,-0.02,-0.12,-0.11,-0.023,0.5,0.0049,-0.1,-0.043,0,0,0.00042,0.00089,0.02,0.12,0.18,0.0088,0.61,0.66,0.037,5.9e-07,5.9e-07,6.4e-06,0.029,0.029,0.0005,0.0012,0.00014,0.0013,0.00018,0.0013,0.0012,0,0 -27490000,0.77,0.064,-0.025,-0.63,2.1,1.5,-1.2,1.7,1.3,-3.7e+02,-0.00074,-0.0058,1.5e-05,0.0096,-0.02,-0.12,-0.11,-0.023,0.5,0.0044,-0.1,-0.043,0,0,0.00039,0.00072,0.02,0.13,0.19,0.0088,0.66,0.72,0.037,5.9e-07,5.9e-07,6.4e-06,0.029,0.029,0.0005,0.0012,0.00012,0.0012,0.00014,0.0012,0.0012,0,0 -27590000,0.77,0.052,-0.022,-0.63,2.2,1.5,-1.2,1.9,1.5,-3.7e+02,-0.00074,-0.0058,1.5e-05,0.0095,-0.02,-0.12,-0.11,-0.024,0.5,0.0039,-0.099,-0.044,0,0,0.00038,0.0006,0.02,0.14,0.2,0.0089,0.71,0.78,0.038,5.9e-07,5.9e-07,6.4e-06,0.029,0.029,0.0005,0.0012,0.00011,0.0012,0.00012,0.0012,0.0012,0,0 -27690000,0.77,0.05,-0.021,-0.63,2.2,1.6,-1.2,2.1,1.7,-3.7e+02,-0.00074,-0.0058,1.5e-05,0.0095,-0.02,-0.12,-0.11,-0.024,0.5,0.0033,-0.096,-0.044,0,0,0.00038,0.00059,0.02,0.14,0.21,0.0089,0.77,0.85,0.038,5.9e-07,5.9e-07,6.4e-06,0.029,0.029,0.0005,0.0011,0.0001,0.0012,0.00011,0.0012,0.0012,0,0 -27790000,0.77,0.052,-0.021,-0.63,2.2,1.6,-1.2,2.4,1.8,-3.7e+02,-0.00074,-0.0058,1.5e-05,0.0094,-0.02,-0.12,-0.12,-0.025,0.49,0.0027,-0.094,-0.044,0,0,0.00038,0.0006,0.02,0.15,0.21,0.009,0.83,0.93,0.038,5.9e-07,5.9e-07,6.4e-06,0.029,0.029,0.0005,0.0011,9.5e-05,0.0011,9.5e-05,0.0012,0.0011,0,0 -27890000,0.77,0.05,-0.021,-0.63,2.3,1.6,-1.2,2.6,2,-3.7e+02,-0.00074,-0.0058,1.5e-05,0.0092,-0.019,-0.12,-0.12,-0.025,0.49,0.0027,-0.092,-0.044,0,0,0.00038,0.00058,0.02,0.15,0.22,0.009,0.89,1,0.038,5.9e-07,5.9e-07,6.4e-06,0.029,0.029,0.0005,0.0011,9.1e-05,0.0011,8.6e-05,0.0011,0.0011,0,0 -27990000,0.77,0.046,-0.02,-0.63,2.3,1.6,-1.2,2.8,2.1,-3.7e+02,-0.00074,-0.0058,1.5e-05,0.0091,-0.019,-0.12,-0.12,-0.025,0.49,0.0025,-0.091,-0.044,0,0,0.00038,0.00056,0.02,0.16,0.23,0.0091,0.96,1.1,0.038,5.9e-07,5.9e-07,6.4e-06,0.029,0.029,0.0005,0.0011,8.6e-05,0.0011,7.8e-05,0.0011,0.0011,0,0 -28090000,0.77,0.06,-0.024,-0.63,2.3,1.6,-1.2,3,2.3,-3.7e+02,-0.00074,-0.0058,1.5e-05,0.009,-0.019,-0.12,-0.12,-0.025,0.49,0.0022,-0.09,-0.044,0,0,0.00039,0.00068,0.02,0.16,0.24,0.0091,1,1.2,0.038,5.9e-07,5.9e-07,6.4e-06,0.029,0.029,0.0005,0.0011,8.3e-05,0.0011,7.1e-05,0.0011,0.0011,0,0 -28190000,0.77,0.073,-0.028,-0.63,2.4,1.7,-0.95,3.3,2.4,-3.7e+02,-0.00073,-0.0058,1.5e-05,0.0088,-0.018,-0.12,-0.12,-0.025,0.49,0.0022,-0.09,-0.044,0,0,0.00041,0.00082,0.02,0.17,0.24,0.0092,1.1,1.3,0.038,5.9e-07,5.9e-07,6.4e-06,0.029,0.029,0.0005,0.0011,8.1e-05,0.0011,6.7e-05,0.0011,0.0011,0,0 -28290000,0.77,0.055,-0.022,-0.63,2.4,1.7,-0.089,3.5,2.6,-3.7e+02,-0.00073,-0.0058,1.5e-05,0.0086,-0.018,-0.12,-0.12,-0.025,0.49,0.0021,-0.09,-0.044,0,0,0.00039,0.00063,0.02,0.17,0.24,0.0093,1.2,1.4,0.038,5.9e-07,6e-07,6.4e-06,0.029,0.029,0.0005,0.0011,7.8e-05,0.0011,6.2e-05,0.0011,0.0011,0,0 -28390000,0.77,0.022,-0.0092,-0.63,2.4,1.7,0.77,3.7,2.8,-3.7e+02,-0.00073,-0.0058,1.5e-05,0.0083,-0.017,-0.12,-0.12,-0.025,0.49,0.0019,-0.089,-0.043,0,0,0.00037,0.00042,0.021,0.17,0.24,0.0093,1.3,1.5,0.038,6e-07,6e-07,6.4e-06,0.029,0.029,0.0005,0.0011,7.5e-05,0.0011,5.8e-05,0.0011,0.0011,0,0 -28490000,0.77,0.0034,-0.0023,-0.63,2.3,1.7,1.1,4,2.9,-3.7e+02,-0.00073,-0.0058,1.5e-05,0.008,-0.016,-0.12,-0.12,-0.026,0.49,0.0017,-0.086,-0.041,0,0,0.00037,0.00038,0.021,0.17,0.24,0.0094,1.4,1.6,0.038,6e-07,6e-07,6.4e-06,0.029,0.029,0.0005,0.001,7.2e-05,0.001,5.4e-05,0.0011,0.001,0,0 -28590000,0.77,-0.00028,-0.00089,-0.63,2.2,1.6,0.96,4.2,3.1,-3.7e+02,-0.00073,-0.0058,1.5e-05,0.0076,-0.015,-0.12,-0.13,-0.027,0.49,0.0016,-0.081,-0.04,0,0,0.00037,0.00038,0.021,0.17,0.24,0.0094,1.5,1.7,0.038,6e-07,6e-07,6.4e-06,0.029,0.029,0.0005,0.00096,6.8e-05,0.00098,5.1e-05,0.001,0.00098,0,0 -28690000,0.77,-0.0013,-0.00027,-0.63,2.2,1.6,0.96,4.4,3.3,-3.7e+02,-0.00072,-0.0058,1.6e-05,0.0072,-0.014,-0.12,-0.13,-0.028,0.48,0.0013,-0.076,-0.038,0,0,0.00038,0.00038,0.021,0.18,0.24,0.0094,1.6,1.9,0.038,6e-07,6e-07,6.4e-06,0.029,0.029,0.0005,0.00092,6.5e-05,0.00093,4.8e-05,0.00095,0.00093,0,0 -28790000,0.77,-0.0016,1.4e-05,-0.63,2.1,1.6,0.97,4.6,3.4,-3.7e+02,-0.00072,-0.0058,1.6e-05,0.0067,-0.013,-0.12,-0.13,-0.028,0.48,0.0011,-0.073,-0.037,0,0,0.00038,0.00038,0.021,0.18,0.24,0.0095,1.7,2,0.038,6e-07,6e-07,6.4e-06,0.029,0.029,0.0005,0.00088,6.2e-05,0.00089,4.6e-05,0.00092,0.00089,0,0 -28890000,0.78,-0.0014,6.6e-05,-0.63,2,1.5,0.95,4.8,3.6,-3.7e+02,-0.00072,-0.0058,1.6e-05,0.0063,-0.012,-0.12,-0.13,-0.029,0.48,0.00095,-0.071,-0.036,0,0,0.00038,0.00038,0.021,0.18,0.24,0.0095,1.8,2.1,0.039,6e-07,6e-07,6.4e-06,0.029,0.029,0.0005,0.00085,6e-05,0.00087,4.4e-05,0.00089,0.00087,0,0 -28990000,0.78,-0.0011,-1.2e-05,-0.63,2,1.5,0.95,5,3.7,-3.7e+02,-0.00072,-0.0058,1.6e-05,0.0057,-0.011,-0.12,-0.14,-0.029,0.48,0.00073,-0.069,-0.034,0,0,0.00038,0.00038,0.021,0.19,0.24,0.0096,1.9,2.3,0.039,6e-07,6e-07,6.4e-06,0.029,0.029,0.0005,0.00083,5.8e-05,0.00084,4.2e-05,0.00086,0.00084,0,0 -29090000,0.78,-0.00065,-0.00011,-0.63,1.9,1.5,0.94,5.2,3.9,-3.7e+02,-0.00071,-0.0058,1.6e-05,0.0053,-0.0093,-0.12,-0.14,-0.029,0.48,0.0006,-0.067,-0.033,0,0,0.00038,0.00038,0.021,0.19,0.24,0.0096,2,2.4,0.039,6e-07,6e-07,6.4e-06,0.029,0.028,0.0005,0.0008,5.6e-05,0.00082,4e-05,0.00084,0.00082,0,0 -29190000,0.78,-0.00033,-0.00021,-0.63,1.8,1.4,0.93,5.4,4,-3.7e+02,-0.00071,-0.0058,1.6e-05,0.0047,-0.008,-0.12,-0.14,-0.03,0.48,0.00061,-0.065,-0.033,0,0,0.00038,0.00038,0.021,0.19,0.24,0.0097,2.1,2.5,0.039,6e-07,6e-07,6.4e-06,0.029,0.028,0.0005,0.00079,5.5e-05,0.0008,3.9e-05,0.00082,0.0008,0,0 -29290000,0.78,0.00054,-0.00045,-0.63,1.8,1.4,0.96,5.6,4.2,-3.7e+02,-0.00071,-0.0058,1.6e-05,0.0041,-0.0064,-0.11,-0.14,-0.03,0.47,0.00048,-0.063,-0.033,0,0,0.00038,0.00039,0.021,0.2,0.24,0.0097,2.2,2.7,0.039,6e-07,6.1e-07,6.4e-06,0.029,0.028,0.0005,0.00077,5.3e-05,0.00078,3.7e-05,0.0008,0.00078,0,0 -29390000,0.78,0.0019,-0.00073,-0.63,1.7,1.4,0.97,5.7,4.3,-3.7e+02,-0.00071,-0.0058,1.6e-05,0.0034,-0.0047,-0.11,-0.14,-0.03,0.47,0.00028,-0.062,-0.032,0,0,0.00038,0.00039,0.021,0.2,0.24,0.0097,2.4,2.9,0.039,6.1e-07,6.1e-07,6.4e-06,0.029,0.028,0.0005,0.00076,5.2e-05,0.00077,3.6e-05,0.00079,0.00077,0,0 -29490000,0.78,0.0031,-0.0011,-0.63,1.7,1.4,0.96,5.9,4.4,-3.7e+02,-0.00071,-0.0058,1.6e-05,0.003,-0.0035,-0.11,-0.14,-0.03,0.47,0.00026,-0.061,-0.032,0,0,0.00039,0.00039,0.021,0.21,0.24,0.0098,2.5,3,0.039,6.1e-07,6.1e-07,6.4e-06,0.029,0.028,0.0005,0.00075,5.1e-05,0.00076,3.5e-05,0.00078,0.00076,0,0 -29590000,0.78,0.0041,-0.0013,-0.63,1.6,1.3,0.96,6.1,4.6,-3.7e+02,-0.0007,-0.0058,1.6e-05,0.0022,-0.0016,-0.11,-0.14,-0.031,0.47,0.00022,-0.06,-0.032,0,0,0.00039,0.00039,0.021,0.21,0.24,0.0098,2.6,3.2,0.039,6.1e-07,6.1e-07,6.4e-06,0.029,0.028,0.0005,0.00074,5e-05,0.00075,3.4e-05,0.00077,0.00075,0,0 -29690000,0.78,0.0048,-0.0015,-0.63,1.6,1.3,0.95,6.2,4.7,-3.7e+02,-0.0007,-0.0058,1.6e-05,0.0017,-0.0002,-0.11,-0.14,-0.031,0.47,0.00016,-0.059,-0.031,0,0,0.00039,0.00039,0.021,0.22,0.25,0.0098,2.8,3.4,0.039,6.1e-07,6.1e-07,6.4e-06,0.029,0.028,0.0005,0.00073,5e-05,0.00074,3.2e-05,0.00076,0.00074,0,0 -29790000,0.78,0.0052,-0.0016,-0.63,1.5,1.3,0.94,6.4,4.8,-3.7e+02,-0.0007,-0.0058,1.6e-05,0.0011,0.0014,-0.11,-0.14,-0.031,0.47,0.00019,-0.059,-0.031,0,0,0.00039,0.00039,0.021,0.22,0.25,0.0099,2.9,3.5,0.039,6.1e-07,6.1e-07,6.4e-06,0.029,0.028,0.0005,0.00072,4.9e-05,0.00073,3.2e-05,0.00075,0.00073,0,0 -29890000,0.78,0.0054,-0.0017,-0.63,1.5,1.3,0.92,6.5,4.9,-3.7e+02,-0.0007,-0.0058,1.6e-05,0.00015,0.0037,-0.11,-0.14,-0.031,0.47,0.00011,-0.058,-0.031,0,0,0.00039,0.0004,0.021,0.23,0.25,0.0099,3.1,3.7,0.039,6.1e-07,6.1e-07,6.4e-06,0.029,0.028,0.0005,0.00072,4.8e-05,0.00073,3.1e-05,0.00075,0.00073,0,0 -29990000,0.78,0.0055,-0.0017,-0.63,1.5,1.3,0.91,6.7,5.1,-3.7e+02,-0.0007,-0.0058,1.6e-05,-0.00057,0.0056,-0.11,-0.14,-0.031,0.47,7.6e-05,-0.058,-0.031,0,0,0.00039,0.0004,0.021,0.24,0.26,0.0099,3.3,3.9,0.039,6.1e-07,6.1e-07,6.4e-06,0.029,0.028,0.0005,0.00071,4.7e-05,0.00072,3e-05,0.00074,0.00072,0,0 -30090000,0.78,0.0054,-0.0017,-0.63,1.4,1.3,0.89,6.8,5.2,-3.7e+02,-0.0007,-0.0058,1.6e-05,-0.0012,0.0073,-0.11,-0.15,-0.031,0.47,1.5e-05,-0.057,-0.031,0,0,0.00039,0.0004,0.021,0.24,0.26,0.0098,3.4,4.1,0.039,6.1e-07,6.1e-07,6.4e-06,0.029,0.028,0.0005,0.00071,4.7e-05,0.00071,2.9e-05,0.00074,0.00071,0,0 -30190000,0.77,0.0052,-0.0016,-0.63,1.4,1.2,0.88,7,5.3,-3.7e+02,-0.0007,-0.0058,1.6e-05,-0.0017,0.0085,-0.11,-0.15,-0.031,0.47,4.5e-05,-0.056,-0.031,0,0,0.00039,0.0004,0.021,0.25,0.27,0.0099,3.6,4.3,0.04,6.1e-07,6.1e-07,6.4e-06,0.029,0.028,0.0005,0.0007,4.6e-05,0.00071,2.8e-05,0.00073,0.00071,0,0 -30290000,0.77,0.005,-0.0015,-0.63,1.3,1.2,0.87,7.1,5.4,-3.7e+02,-0.0007,-0.0058,1.6e-05,-0.0021,0.0095,-0.11,-0.15,-0.032,0.47,2.3e-05,-0.056,-0.031,0,0,0.00039,0.0004,0.021,0.26,0.27,0.0099,3.8,4.5,0.04,6.1e-07,6.2e-07,6.4e-06,0.029,0.028,0.0005,0.0007,4.6e-05,0.0007,2.8e-05,0.00073,0.0007,0,0 -30390000,0.77,0.0047,-0.0015,-0.63,1.3,1.2,0.85,7.2,5.6,-3.7e+02,-0.0007,-0.0058,1.5e-05,-0.0028,0.011,-0.11,-0.15,-0.032,0.47,1.3e-05,-0.055,-0.031,0,0,0.0004,0.0004,0.021,0.26,0.28,0.0099,4,4.7,0.039,6.1e-07,6.2e-07,6.4e-06,0.029,0.028,0.0005,0.00069,4.5e-05,0.0007,2.7e-05,0.00072,0.0007,0,0 -30490000,0.77,0.0044,-0.0013,-0.63,1.3,1.2,0.84,7.4,5.7,-3.7e+02,-0.0007,-0.0058,1.5e-05,-0.0032,0.012,-0.11,-0.15,-0.032,0.47,3.3e-05,-0.055,-0.031,0,0,0.0004,0.0004,0.021,0.27,0.29,0.0099,4.2,5,0.04,6.2e-07,6.2e-07,6.4e-06,0.029,0.028,0.0005,0.00069,4.5e-05,0.0007,2.6e-05,0.00072,0.0007,0,0 -30590000,0.77,0.004,-0.0012,-0.63,1.2,1.2,0.8,7.5,5.8,-3.7e+02,-0.0007,-0.0058,1.5e-05,-0.0039,0.014,-0.11,-0.15,-0.032,0.47,7.8e-05,-0.055,-0.031,0,0,0.0004,0.00041,0.021,0.28,0.29,0.0099,4.4,5.2,0.04,6.2e-07,6.2e-07,6.4e-06,0.029,0.028,0.0005,0.00068,4.4e-05,0.00069,2.6e-05,0.00071,0.00069,0,0 -30690000,0.77,0.0037,-0.0011,-0.63,1.2,1.2,0.79,7.6,5.9,-3.7e+02,-0.0007,-0.0058,1.5e-05,-0.0045,0.015,-0.11,-0.15,-0.032,0.47,7.6e-05,-0.054,-0.031,0,0,0.0004,0.00041,0.021,0.29,0.3,0.0099,4.6,5.4,0.04,6.2e-07,6.2e-07,6.4e-06,0.028,0.028,0.0005,0.00068,4.4e-05,0.00069,2.5e-05,0.00071,0.00069,0,0 -30790000,0.77,0.0033,-0.00093,-0.63,1.1,1.1,0.79,7.7,6,-3.7e+02,-0.0007,-0.0058,1.5e-05,-0.0048,0.016,-0.11,-0.15,-0.032,0.47,3.4e-05,-0.054,-0.03,0,0,0.0004,0.00041,0.021,0.29,0.31,0.0099,4.8,5.7,0.04,6.2e-07,6.2e-07,6.4e-06,0.028,0.028,0.0005,0.00068,4.4e-05,0.00068,2.5e-05,0.0007,0.00068,0,0 -30890000,0.77,0.0028,-0.00078,-0.63,1.1,1.1,0.77,7.8,6.1,-3.7e+02,-0.0007,-0.0058,1.5e-05,-0.0053,0.018,-0.11,-0.15,-0.032,0.47,4.4e-05,-0.053,-0.03,0,0,0.0004,0.00041,0.021,0.3,0.31,0.0099,5.1,5.9,0.04,6.2e-07,6.2e-07,6.4e-06,0.028,0.028,0.0005,0.00067,4.3e-05,0.00068,2.4e-05,0.0007,0.00068,0,0 -30990000,0.77,0.0023,-0.00066,-0.63,1.1,1.1,0.76,7.9,6.3,-3.7e+02,-0.0007,-0.0058,1.5e-05,-0.0059,0.019,-0.11,-0.15,-0.032,0.47,4.4e-05,-0.053,-0.03,0,0,0.0004,0.00041,0.021,0.31,0.32,0.0099,5.3,6.2,0.04,6.2e-07,6.2e-07,6.4e-06,0.028,0.027,0.0005,0.00067,4.3e-05,0.00068,2.3e-05,0.0007,0.00068,0,0 -31090000,0.77,0.0018,-0.00049,-0.63,1,1.1,0.75,8,6.4,-3.7e+02,-0.0007,-0.0058,1.5e-05,-0.0066,0.021,-0.1,-0.15,-0.032,0.47,2.3e-05,-0.053,-0.03,0,0,0.0004,0.00041,0.021,0.32,0.33,0.0099,5.6,6.5,0.04,6.2e-07,6.2e-07,6.4e-06,0.028,0.027,0.0005,0.00066,4.2e-05,0.00067,2.3e-05,0.00069,0.00067,0,0 -31190000,0.77,0.0014,-0.00036,-0.63,0.99,1.1,0.74,8.1,6.5,-3.7e+02,-0.0007,-0.0058,1.5e-05,-0.0076,0.023,-0.1,-0.15,-0.032,0.47,2.8e-05,-0.053,-0.03,0,0,0.0004,0.00041,0.021,0.33,0.34,0.0099,5.8,6.7,0.04,6.2e-07,6.2e-07,6.4e-06,0.028,0.027,0.0005,0.00066,4.2e-05,0.00067,2.3e-05,0.00069,0.00067,0,0 -31290000,0.77,0.00091,-0.00018,-0.63,0.95,1.1,0.74,8.2,6.6,-3.7e+02,-0.0007,-0.0058,1.4e-05,-0.0084,0.025,-0.1,-0.15,-0.032,0.47,3.7e-05,-0.052,-0.03,0,0,0.0004,0.00042,0.022,0.34,0.34,0.0098,6.1,7,0.04,6.2e-07,6.3e-07,6.4e-06,0.028,0.027,0.0005,0.00066,4.2e-05,0.00067,2.2e-05,0.00069,0.00067,0,0 -31390000,0.77,0.00027,1.6e-05,-0.63,0.92,1.1,0.74,8.3,6.7,-3.7e+02,-0.00071,-0.0058,1.4e-05,-0.0089,0.027,-0.1,-0.15,-0.032,0.47,3.7e-05,-0.052,-0.031,0,0,0.0004,0.00042,0.022,0.35,0.35,0.0098,6.4,7.3,0.04,6.2e-07,6.3e-07,6.4e-06,0.028,0.027,0.0005,0.00065,4.1e-05,0.00066,2.2e-05,0.00068,0.00066,0,0 -31490000,0.77,-0.00035,0.00017,-0.63,0.88,1,0.74,8.4,6.8,-3.7e+02,-0.00071,-0.0058,1.4e-05,-0.0096,0.029,-0.1,-0.15,-0.032,0.47,-5.2e-06,-0.051,-0.031,0,0,0.00041,0.00042,0.022,0.36,0.36,0.0098,6.7,7.6,0.04,6.3e-07,6.3e-07,6.4e-06,0.028,0.027,0.0005,0.00065,4.1e-05,0.00066,2.1e-05,0.00068,0.00066,0,0 -31590000,0.77,-0.00077,0.00031,-0.63,0.84,1,0.73,8.5,6.9,-3.7e+02,-0.00071,-0.0058,1.4e-05,-0.01,0.03,-0.1,-0.15,-0.032,0.47,1.8e-05,-0.051,-0.03,0,0,0.00041,0.00042,0.022,0.37,0.37,0.0098,7,7.9,0.04,6.3e-07,6.3e-07,6.4e-06,0.028,0.027,0.0005,0,0,0,0,0,0,0,0 -31690000,0.77,-0.0014,0.00051,-0.63,0.8,1,0.74,8.6,7,-3.7e+02,-0.00071,-0.0058,1.4e-05,-0.011,0.032,-0.1,-0.15,-0.032,0.47,1.8e-05,-0.051,-0.03,0,0,0.00041,0.00042,0.022,0.38,0.38,0.0097,7.3,8.3,0.04,6.3e-07,6.3e-07,6.4e-06,0.028,0.027,0.0005,0,0,0,0,0,0,0,0 -31790000,0.77,-0.0022,0.00077,-0.63,0.77,0.99,0.74,8.7,7.1,-3.7e+02,-0.00071,-0.0058,1.4e-05,-0.012,0.033,-0.1,-0.15,-0.032,0.47,1.8e-05,-0.051,-0.03,0,0,0.00041,0.00043,0.022,0.39,0.39,0.0098,7.6,8.6,0.04,6.3e-07,6.3e-07,6.4e-06,0.028,0.027,0.0005,0,0,0,0,0,0,0,0 -31890000,0.77,-0.0029,0.00095,-0.63,0.73,0.97,0.73,8.8,7.2,-3.7e+02,-0.00071,-0.0058,1.3e-05,-0.012,0.035,-0.1,-0.15,-0.032,0.47,1.8e-05,-0.051,-0.03,0,0,0.00041,0.00043,0.022,0.4,0.4,0.0097,7.9,8.9,0.04,6.3e-07,6.3e-07,6.4e-06,0.028,0.027,0.0005,0,0,0,0,0,0,0,0 -31990000,0.77,-0.0034,0.0011,-0.63,0.69,0.95,0.73,8.9,7.3,-3.7e+02,-0.00072,-0.0058,1.3e-05,-0.013,0.037,-0.1,-0.15,-0.032,0.47,1.8e-05,-0.051,-0.03,0,0,0.00041,0.00043,0.022,0.41,0.41,0.0097,8.3,9.3,0.04,6.3e-07,6.3e-07,6.4e-06,0.028,0.027,0.0005,0,0,0,0,0,0,0,0 -32090000,0.77,-0.0042,0.0013,-0.63,0.65,0.93,0.73,8.9,7.4,-3.7e+02,-0.00072,-0.0058,1.3e-05,-0.014,0.039,-0.1,-0.15,-0.032,0.47,1.8e-05,-0.051,-0.03,0,0,0.00041,0.00043,0.022,0.42,0.42,0.0097,8.6,9.7,0.04,6.3e-07,6.3e-07,6.4e-06,0.028,0.027,0.0005,0,0,0,0,0,0,0,0 -32190000,0.77,-0.005,0.0016,-0.63,0.61,0.92,0.73,9,7.5,-3.7e+02,-0.00072,-0.0058,1.3e-05,-0.015,0.041,-0.099,-0.15,-0.032,0.47,1.8e-05,-0.051,-0.03,0,0,0.00041,0.00044,0.022,0.43,0.43,0.0096,9,10,0.04,6.3e-07,6.3e-07,6.4e-06,0.028,0.027,0.0005,0,0,0,0,0,0,0,0 -32290000,0.77,-0.0058,0.0017,-0.63,0.57,0.9,0.73,9.1,7.6,-3.7e+02,-0.00072,-0.0058,1.2e-05,-0.016,0.044,-0.099,-0.15,-0.032,0.47,1.8e-05,-0.051,-0.03,0,0,0.00041,0.00044,0.022,0.44,0.44,0.0096,9.4,10,0.04,6.3e-07,6.4e-07,6.4e-06,0.028,0.026,0.0005,0,0,0,0,0,0,0,0 -32390000,0.77,-0.0064,0.0019,-0.63,0.53,0.88,0.73,9.1,7.7,-3.7e+02,-0.00072,-0.0058,1.2e-05,-0.016,0.044,-0.098,-0.15,-0.032,0.47,1.8e-05,-0.051,-0.03,0,0,0.00041,0.00044,0.022,0.46,0.45,0.0096,9.8,11,0.04,6.3e-07,6.4e-07,6.4e-06,0.028,0.026,0.0005,0,0,0,0,0,0,0,0 -32490000,0.77,-0.0067,0.002,-0.63,0.49,0.86,0.73,9.2,7.8,-3.7e+02,-0.00073,-0.0058,1.2e-05,-0.017,0.046,-0.098,-0.15,-0.032,0.47,1.8e-05,-0.051,-0.03,0,0,0.00042,0.00044,0.022,0.47,0.47,0.0096,10,11,0.04,6.3e-07,6.4e-07,6.4e-06,0.028,0.026,0.0005,0,0,0,0,0,0,0,0 -32590000,0.77,-0.007,0.0021,-0.63,-1.6,-0.84,0.64,-1e+06,1.2e+04,-3.7e+02,-0.00073,-0.0058,1.2e-05,-0.017,0.046,-0.098,-0.15,-0.032,0.47,1.8e-05,-0.051,-0.03,0,0,0.00042,0.00045,0.022,0.25,0.25,0.56,0.25,0.25,0.042,6.4e-07,6.4e-07,6.4e-06,0.028,0.026,0.0005,0,0,0,0,0,0,0,0 -32690000,0.77,-0.0071,0.0021,-0.63,-1.6,-0.86,0.63,-1e+06,1.2e+04,-3.7e+02,-0.00073,-0.0058,1.2e-05,-0.017,0.047,-0.097,-0.15,-0.032,0.47,1.8e-05,-0.051,-0.03,0,0,0.00042,0.00045,0.022,0.25,0.25,0.55,0.26,0.26,0.052,6.4e-07,6.4e-07,6.4e-06,0.028,0.026,0.0005,0,0,0,0,0,0,0,0 -32790000,0.77,-0.0071,0.0021,-0.63,-1.6,-0.84,0.63,-1e+06,1.2e+04,-3.7e+02,-0.00074,-0.0058,1e-05,-0.018,0.048,-0.097,-0.15,-0.032,0.47,1.8e-05,-0.051,-0.03,0,0,0.00042,0.00045,0.022,0.13,0.13,0.27,0.26,0.26,0.052,6.4e-07,6.4e-07,6.4e-06,0.028,0.026,0.0005,0,0,0,0,0,0,0,0 -32890000,0.77,-0.007,0.002,-0.63,-1.6,-0.85,0.62,-1e+06,1.2e+04,-3.7e+02,-0.00074,-0.0058,1e-05,-0.018,0.049,-0.097,-0.15,-0.032,0.47,1.8e-05,-0.051,-0.03,0,0,0.00042,0.00045,0.022,0.13,0.13,0.26,0.27,0.27,0.061,6.4e-07,6.4e-07,6.4e-06,0.028,0.026,0.0005,0,0,0,0,0,0,0,0 -32990000,0.78,-0.007,0.002,-0.63,-1.6,-0.84,0.62,-1e+06,1.2e+04,-3.7e+02,-0.00075,-0.0058,8.3e-06,-0.018,0.05,-0.097,-0.15,-0.032,0.47,1.8e-05,-0.051,-0.03,0,0,0.00042,0.00045,0.022,0.085,0.085,0.17,0.27,0.27,0.059,6.4e-07,6.4e-07,6.4e-06,0.028,0.026,0.0005,0,0,0,0,0,0,0,0 -33090000,0.78,-0.0071,0.002,-0.63,-1.6,-0.86,0.61,-1e+06,1.2e+04,-3.7e+02,-0.00075,-0.0058,8.2e-06,-0.018,0.05,-0.097,-0.15,-0.032,0.47,1.8e-05,-0.051,-0.03,0,0,0.00042,0.00046,0.022,0.086,0.086,0.16,0.28,0.28,0.067,6.4e-07,6.4e-07,6.4e-06,0.028,0.026,0.0005,0,0,0,0,0,0,0,0 -33190000,0.78,-0.0056,-0.0013,-0.63,-1.6,-0.85,0.55,-1e+06,1.2e+04,-3.7e+02,-0.00078,-0.0058,2.6e-06,-0.018,0.051,-0.097,-0.15,-0.032,0.47,1.8e-05,-0.051,-0.03,0,0,0.00043,0.00045,0.022,0.065,0.065,0.11,0.28,0.28,0.063,6.4e-07,6.4e-07,6.4e-06,0.028,0.026,0.0005,0,0,0,0,0,0,0,0 -33290000,0.82,-0.0033,-0.013,-0.57,-1.6,-0.87,0.54,-1e+06,1.2e+04,-3.7e+02,-0.00078,-0.0058,2.6e-06,-0.018,0.051,-0.097,-0.15,-0.032,0.47,-0.00011,-0.052,-0.032,0,0,0.00044,0.00045,0.022,0.066,0.067,0.11,0.29,0.29,0.068,6.4e-07,6.4e-07,6.4e-06,0.028,0.026,0.0005,0.0004,3e-05,0.00041,2e-05,0.00042,0.00041,0,0 -33390000,0.89,-0.0032,-0.011,-0.46,-1.5,-0.86,0.73,-1e+06,1.2e+04,-3.7e+02,-0.00081,-0.0058,-3.3e-06,-0.019,0.052,-0.096,-0.15,-0.033,0.47,-0.00061,-0.049,-0.032,0,0,0.00043,0.00046,0.022,0.053,0.054,0.083,0.29,0.29,0.066,6.3e-07,6.4e-07,6.4e-06,0.028,0.026,0.0005,0.00035,2.8e-05,0.00037,1.9e-05,0.00036,0.00037,0,0 -33490000,0.95,-0.0011,-0.0035,-0.31,-1.6,-0.87,0.75,-1e+06,1.2e+04,-3.7e+02,-0.00081,-0.0058,-3.3e-06,-0.019,0.052,-0.096,-0.16,-0.034,0.47,-0.0015,-0.039,-0.031,0,0,0.00043,0.00046,0.022,0.055,0.056,0.076,0.3,0.3,0.068,6.4e-07,6.4e-07,6.4e-06,0.028,0.026,0.0005,0.00027,2.5e-05,0.00036,1.9e-05,0.00028,0.00036,0,0 -33590000,0.99,-0.0031,0.002,-0.15,-1.5,-0.84,0.71,-1e+06,1.2e+04,-3.7e+02,-0.00087,-0.0059,-8.7e-06,-0.019,0.052,-0.096,-0.18,-0.036,0.47,-0.0019,-0.027,-0.031,0,0,0.00045,0.00046,0.022,0.047,0.048,0.062,0.3,0.3,0.065,6.3e-07,6.3e-07,6.4e-06,0.028,0.026,0.0005,0.00019,2.2e-05,0.00035,1.9e-05,0.0002,0.00035,0,0 -33690000,1,-0.0054,0.0076,0.039,-1.6,-0.87,0.72,-1e+06,1.2e+04,-3.7e+02,-0.00078,-0.0059,-0.00033,-0.019,0.052,-0.096,-0.18,-0.038,0.47,-0.002,-0.022,-0.031,0,0,0.00046,0.00038,0.011,0.049,0.052,0.057,0.31,0.31,0.068,6e-07,6.3e-07,5.8e-06,0.028,0.026,0.0005,0.00014,1.9e-05,0.00034,1.9e-05,0.00016,0.00034,0,0 -33790000,0.98,-0.0057,0.01,0.21,-1.6,-0.87,0.7,-1e+06,1.2e+04,-3.7e+02,-0.00078,-0.0059,-0.00036,-0.019,0.052,-0.096,-0.19,-0.039,0.47,-0.002,-0.018,-0.032,0,0,0.00046,0.00031,0.0078,0.043,0.047,0.047,0.31,0.31,0.064,5.8e-07,6.2e-07,5.6e-06,0.028,0.026,0.0005,0.00011,1.7e-05,0.00034,1.8e-05,0.00013,0.00034,0,0 -33890000,0.93,-0.0054,0.012,0.37,-1.7,-0.91,0.69,-1e+06,1.2e+04,-3.7e+02,-0.00074,-0.0059,-0.0003,-0.019,0.052,-0.096,-0.19,-0.04,0.47,-0.0018,-0.015,-0.032,0,0,0.00045,0.00032,0.0065,0.046,0.053,0.043,0.32,0.32,0.065,5.7e-07,6.2e-07,5.5e-06,0.028,0.026,0.0005,9e-05,1.5e-05,0.00034,1.8e-05,0.00011,0.00034,0,0 -33990000,0.87,-0.0077,0.01,0.5,-1.8,-0.97,0.66,-1e+06,1.2e+04,-3.7e+02,-0.00072,-0.0059,-0.0002,-0.019,0.052,-0.096,-0.19,-0.041,0.47,-0.0015,-0.011,-0.032,0,0,0.00041,0.00034,0.0055,0.05,0.059,0.04,0.33,0.33,0.065,5.7e-07,6.2e-07,5.5e-06,0.028,0.026,0.0005,7.5e-05,1.4e-05,0.00034,1.8e-05,8.8e-05,0.00034,0,0 -34090000,0.8,-0.0085,0.0084,0.59,-1.8,-1,0.67,-1e+06,1.2e+04,-3.7e+02,-0.00072,-0.0059,-0.00011,-0.019,0.052,-0.096,-0.19,-0.041,0.47,-0.0013,-0.0085,-0.032,0,0,0.00037,0.00037,0.0049,0.055,0.067,0.036,0.34,0.34,0.066,5.7e-07,6.2e-07,5.4e-06,0.028,0.026,0.0005,6.7e-05,1.3e-05,0.00034,1.8e-05,7.4e-05,0.00034,0,0 -34190000,0.75,-0.0066,0.0085,0.66,-1.9,-1.1,0.68,-1e+06,1.2e+04,-3.7e+02,-0.00072,-0.0059,-6.8e-05,-0.019,0.052,-0.096,-0.2,-0.041,0.47,-0.0011,-0.0068,-0.032,0,0,0.00034,0.0004,0.0044,0.06,0.076,0.033,0.35,0.36,0.066,5.7e-07,6.2e-07,5.4e-06,0.028,0.026,0.0005,6.1e-05,1.3e-05,0.00034,1.7e-05,6.2e-05,0.00034,0,0 -34290000,0.72,-0.0036,0.0096,0.69,-2,-1.2,0.68,-1e+06,1.2e+04,-3.7e+02,-0.00072,-0.0059,-3.6e-05,-0.019,0.052,-0.096,-0.2,-0.042,0.47,-0.00088,-0.0057,-0.032,0,0,0.00032,0.00042,0.0041,0.067,0.086,0.03,0.37,0.37,0.065,5.7e-07,6.2e-07,5.4e-06,0.028,0.026,0.0005,5.7e-05,1.2e-05,0.00034,1.7e-05,5.4e-05,0.00034,0,0 -34390000,0.7,-0.00069,0.011,0.72,-2,-1.2,0.68,-1e+06,1.2e+04,-3.7e+02,-0.00073,-0.0059,-1.2e-05,-0.019,0.052,-0.096,-0.2,-0.042,0.47,-0.00073,-0.0049,-0.032,0,0,0.00031,0.00044,0.0039,0.075,0.098,0.028,0.38,0.39,0.065,5.7e-07,6.2e-07,5.4e-06,0.028,0.026,0.0005,5.4e-05,1.2e-05,0.00034,1.6e-05,4.9e-05,0.00034,0,0 -34490000,0.68,0.0014,0.012,0.73,-2.1,-1.3,0.68,-1e+06,1.2e+04,-3.7e+02,-0.00073,-0.0059,1.9e-06,-0.019,0.052,-0.096,-0.2,-0.042,0.47,-0.00066,-0.0044,-0.032,0,0,0.0003,0.00045,0.0037,0.084,0.11,0.026,0.4,0.41,0.064,5.7e-07,6.2e-07,5.4e-06,0.028,0.026,0.0005,5.2e-05,1.2e-05,0.00033,1.6e-05,4.4e-05,0.00034,0,0 -34590000,0.67,0.0027,0.013,0.74,-2.1,-1.3,0.68,-1e+06,1.2e+04,-3.7e+02,-0.00073,-0.0059,2e-05,-0.019,0.052,-0.096,-0.2,-0.042,0.47,-0.00061,-0.004,-0.032,0,0,0.00029,0.00046,0.0036,0.094,0.13,0.024,0.42,0.43,0.062,5.7e-07,6.2e-07,5.4e-06,0.028,0.026,0.0005,5e-05,1.2e-05,0.00033,1.6e-05,4.1e-05,0.00034,0,0 -34690000,0.67,0.0034,0.013,0.74,-2.2,-1.4,0.69,-1e+06,1.2e+04,-3.7e+02,-0.00073,-0.0059,2e-05,-0.019,0.052,-0.096,-0.2,-0.042,0.47,-0.00046,-0.0036,-0.032,0,0,0.00029,0.00046,0.0035,0.11,0.14,0.022,0.44,0.46,0.062,5.7e-07,6.2e-07,5.4e-06,0.028,0.026,0.0005,4.9e-05,1.2e-05,0.00033,1.6e-05,3.9e-05,0.00034,0,0 -34790000,0.66,0.004,0.013,0.75,-2.2,-1.5,0.69,-1e+06,1.2e+04,-3.7e+02,-0.00073,-0.0059,8.7e-06,-0.019,0.052,-0.096,-0.2,-0.042,0.47,-0.00036,-0.0036,-0.032,0,0,0.00028,0.00046,0.0034,0.12,0.16,0.02,0.47,0.49,0.061,5.7e-07,6.3e-07,5.3e-06,0.028,0.026,0.0005,4.8e-05,1.1e-05,0.00033,1.5e-05,3.7e-05,0.00033,0,0 -34890000,0.66,0.0041,0.013,0.75,-2.3,-1.5,0.69,-1e+06,1.2e+04,-3.7e+02,-0.00073,-0.0059,1.7e-05,-0.019,0.052,-0.096,-0.2,-0.042,0.47,-0.00035,-0.0034,-0.032,0,0,0.00028,0.00046,0.0033,0.13,0.18,0.019,0.5,0.52,0.059,5.7e-07,6.3e-07,5.3e-06,0.028,0.026,0.0005,4.8e-05,1.1e-05,0.00033,1.5e-05,3.6e-05,0.00033,0,0 -34990000,0.66,0.00075,0.021,0.75,-3.3,-2.4,-0.12,-1e+06,1.2e+04,-3.7e+02,-0.00073,-0.0059,2.9e-06,-0.019,0.052,-0.096,-0.2,-0.042,0.47,-0.0003,-0.0034,-0.032,0,0,0.00028,0.00047,0.0033,0.16,0.25,0.018,0.53,0.56,0.059,5.7e-07,6.3e-07,5.3e-06,0.028,0.026,0.0005,4.7e-05,1.1e-05,0.00033,1.5e-05,3.4e-05,0.00033,0,0 -35090000,0.66,0.00067,0.021,0.75,-3.4,-2.5,-0.17,-1e+06,1.2e+04,-3.7e+02,-0.00073,-0.0059,9.9e-07,-0.019,0.052,-0.096,-0.2,-0.042,0.47,-0.00029,-0.0034,-0.032,0,0,0.00028,0.00047,0.0032,0.17,0.27,0.017,0.56,0.61,0.057,5.7e-07,6.3e-07,5.3e-06,0.028,0.026,0.0005,4.6e-05,1.1e-05,0.00033,1.5e-05,3.3e-05,0.00033,0,0 -35190000,0.66,0.00055,0.021,0.75,-3.4,-2.5,-0.16,-1e+06,1.2e+04,-3.7e+02,-0.00073,-0.0059,-4.5e-06,-0.019,0.052,-0.096,-0.2,-0.042,0.47,-0.00025,-0.0033,-0.032,0,0,0.00028,0.00047,0.0032,0.18,0.29,0.016,0.6,0.67,0.056,5.7e-07,6.3e-07,5.3e-06,0.028,0.026,0.0005,4.6e-05,1.1e-05,0.00033,1.5e-05,3.3e-05,0.00033,0,0 -35290000,0.66,0.00042,0.021,0.75,-3.5,-2.6,-0.15,-1e+06,1.2e+04,-3.7e+02,-0.00073,-0.0059,-1.3e-05,-0.019,0.052,-0.096,-0.2,-0.042,0.47,-0.00021,-0.0033,-0.032,0,0,0.00028,0.00047,0.0031,0.19,0.31,0.015,0.65,0.73,0.055,5.8e-07,6.3e-07,5.3e-06,0.028,0.026,0.0005,4.5e-05,1.1e-05,0.00033,1.5e-05,3.2e-05,0.00033,0,0 -35390000,0.66,0.00042,0.021,0.75,-3.5,-2.6,-0.15,-1e+06,1.2e+04,-3.7e+02,-0.00073,-0.0059,-2.7e-05,-0.019,0.052,-0.096,-0.2,-0.042,0.47,-0.00019,-0.0034,-0.032,0,0,0.00027,0.00046,0.0031,0.21,0.33,0.014,0.7,0.8,0.054,5.8e-07,6.3e-07,5.3e-06,0.028,0.026,0.0005,4.5e-05,1.1e-05,0.00033,1.5e-05,3.1e-05,0.00033,0,0 -35490000,0.66,0.00037,0.021,0.75,-3.6,-2.7,-0.14,-1e+06,1.2e+04,-3.7e+02,-0.00073,-0.0059,-4.1e-05,-0.019,0.052,-0.096,-0.2,-0.042,0.47,-0.00017,-0.0034,-0.032,0,0,0.00027,0.00046,0.0031,0.22,0.36,0.013,0.75,0.88,0.053,5.8e-07,6.3e-07,5.3e-06,0.028,0.026,0.0005,4.5e-05,1.1e-05,0.00033,1.5e-05,3.1e-05,0.00033,0,0 -35590000,0.66,0.00032,0.021,0.75,-3.6,-2.7,-0.14,-1e+06,1.2e+04,-3.7e+02,-0.00073,-0.0059,-3.4e-05,-0.019,0.052,-0.096,-0.2,-0.042,0.47,-0.00016,-0.0033,-0.032,0,0,0.00027,0.00046,0.0031,0.23,0.38,0.013,0.82,0.97,0.052,5.8e-07,6.3e-07,5.3e-06,0.028,0.026,0.0005,4.4e-05,1.1e-05,0.00033,1.5e-05,3e-05,0.00033,0,0 -35690000,0.66,0.00034,0.021,0.75,-3.6,-2.8,-0.13,-1e+06,1.2e+04,-3.7e+02,-0.00073,-0.0059,-4.8e-05,-0.019,0.052,-0.096,-0.2,-0.042,0.47,-0.00014,-0.0034,-0.032,0,0,0.00027,0.00046,0.003,0.25,0.4,0.012,0.88,1.1,0.051,5.8e-07,6.3e-07,5.3e-06,0.028,0.026,0.0005,4.4e-05,1.1e-05,0.00033,1.5e-05,3e-05,0.00033,0,0 -35790000,0.66,0.00028,0.021,0.75,-3.7,-2.8,-0.12,-1e+06,1.2e+04,-3.7e+02,-0.00073,-0.0059,-5.2e-05,-0.019,0.052,-0.096,-0.2,-0.042,0.47,-0.00012,-0.0034,-0.032,0,0,0.00027,0.00046,0.003,0.26,0.43,0.012,0.96,1.2,0.05,5.8e-07,6.3e-07,5.3e-06,0.028,0.026,0.0005,4.4e-05,1e-05,0.00033,1.5e-05,2.9e-05,0.00033,0,0 -35890000,0.66,0.00018,0.021,0.75,-3.7,-2.9,-0.12,-1e+06,1.2e+04,-3.7e+02,-0.00073,-0.0059,-5.4e-05,-0.019,0.052,-0.096,-0.2,-0.042,0.47,-0.00011,-0.0033,-0.032,0,0,0.00027,0.00046,0.003,0.28,0.45,0.011,1,1.3,0.049,5.8e-07,6.3e-07,5.3e-06,0.028,0.026,0.0005,4.4e-05,1e-05,0.00033,1.5e-05,2.9e-05,0.00033,0,0 -35990000,0.66,0.00015,0.021,0.75,-3.8,-2.9,-0.11,-1e+06,1.2e+04,-3.7e+02,-0.00073,-0.0059,-4.7e-05,-0.019,0.052,-0.096,-0.2,-0.042,0.47,-0.00011,-0.0033,-0.032,0,0,0.00027,0.00045,0.003,0.3,0.48,0.011,1.1,1.4,0.048,5.8e-07,6.4e-07,5.3e-06,0.028,0.026,0.0005,4.3e-05,1e-05,0.00033,1.5e-05,2.9e-05,0.00033,0,0 -36090000,0.66,0.00011,0.021,0.75,-3.8,-3,-0.1,-1e+06,1.2e+04,-3.7e+02,-0.00073,-0.0059,-5.6e-05,-0.019,0.052,-0.096,-0.2,-0.042,0.47,-0.0001,-0.0033,-0.032,0,0,0.00027,0.00045,0.003,0.32,0.51,0.01,1.2,1.6,0.047,5.8e-07,6.4e-07,5.2e-06,0.028,0.026,0.0005,4.3e-05,1e-05,0.00033,1.5e-05,2.9e-05,0.00033,0,0 -36190000,0.66,5.5e-05,0.021,0.75,-3.8,-3.1,-0.096,-1e+06,1.2e+04,-3.7e+02,-0.00072,-0.0059,-8.4e-05,-0.019,0.052,-0.096,-0.2,-0.042,0.47,-7.5e-05,-0.0034,-0.032,0,0,0.00027,0.00045,0.003,0.34,0.54,0.0099,1.3,1.7,0.046,5.8e-07,6.4e-07,5.2e-06,0.028,0.026,0.0005,4.3e-05,1e-05,0.00033,1.4e-05,2.8e-05,0.00033,0,0 -36290000,0.66,5.7e-05,0.021,0.75,-3.9,-3.1,-0.088,-1e+06,1.2e+04,-3.7e+02,-0.00072,-0.0059,-9e-05,-0.019,0.052,-0.096,-0.2,-0.042,0.47,-6.2e-05,-0.0034,-0.032,0,0,0.00027,0.00045,0.003,0.36,0.57,0.0097,1.4,1.9,0.046,5.8e-07,6.4e-07,5.2e-06,0.028,0.026,0.0005,4.3e-05,1e-05,0.00033,1.4e-05,2.8e-05,0.00033,0,0 -36390000,0.66,-2.5e-05,0.021,0.75,-3.9,-3.2,-0.082,-1e+06,1.2e+04,-3.7e+02,-0.00072,-0.0059,-9e-05,-0.019,0.052,-0.097,-0.2,-0.042,0.47,-3.9e-05,-0.0033,-0.032,0,0,0.00026,0.00045,0.0029,0.38,0.61,0.0094,1.6,2.1,0.045,5.8e-07,6.4e-07,5.2e-06,0.028,0.026,0.0005,4.3e-05,1e-05,0.00033,1.4e-05,2.8e-05,0.00033,0,0 -36490000,0.66,-0.00011,0.021,0.75,-4,-3.2,-0.077,-1e+06,1.2e+04,-3.7e+02,-0.00072,-0.0059,-7.9e-05,-0.019,0.052,-0.097,-0.2,-0.042,0.47,-3.5e-05,-0.0033,-0.032,0,0,0.00026,0.00045,0.0029,0.4,0.64,0.0092,1.7,2.3,0.044,5.8e-07,6.4e-07,5.2e-06,0.028,0.026,0.0005,4.3e-05,1e-05,0.00033,1.4e-05,2.8e-05,0.00033,0,0 -36590000,0.66,-0.00013,0.021,0.75,-4,-3.3,-0.068,-1e+06,1.2e+04,-3.7e+02,-0.00072,-0.0059,-9.7e-05,-0.019,0.052,-0.097,-0.2,-0.042,0.47,-6.4e-06,-0.0033,-0.032,0,0,0.00026,0.00045,0.0029,0.42,0.68,0.009,1.8,2.5,0.044,5.8e-07,6.4e-07,5.2e-06,0.028,0.026,0.0005,4.2e-05,1e-05,0.00033,1.4e-05,2.7e-05,0.00033,0,0 -36690000,0.66,-0.00016,0.021,0.75,-4,-3.3,-0.062,-1e+06,1.2e+04,-3.7e+02,-0.00072,-0.0058,-0.00011,-0.019,0.052,-0.097,-0.2,-0.042,0.47,1e-05,-0.0033,-0.032,0,0,0.00026,0.00044,0.0029,0.45,0.71,0.0089,2,2.8,0.043,5.8e-07,6.4e-07,5.2e-06,0.028,0.026,0.0005,4.2e-05,9.9e-06,0.00033,1.4e-05,2.7e-05,0.00033,0,0 -36790000,0.66,-0.00021,0.021,0.75,-4.1,-3.4,-0.054,-1e+06,1.2e+04,-3.7e+02,-0.00072,-0.0058,-0.00013,-0.019,0.051,-0.097,-0.2,-0.042,0.47,1.2e-05,-0.0033,-0.032,0,0,0.00026,0.00044,0.0029,0.47,0.75,0.0088,2.2,3,0.043,5.8e-07,6.4e-07,5.2e-06,0.028,0.026,0.0005,4.2e-05,9.9e-06,0.00033,1.4e-05,2.7e-05,0.00033,0,0 -36890000,0.66,-0.00026,0.021,0.75,-4.1,-3.5,-0.048,-1e+06,1.2e+04,-3.7e+02,-0.00072,-0.0058,-0.00015,-0.018,0.051,-0.097,-0.2,-0.042,0.47,2.5e-05,-0.0033,-0.032,0,0,0.00026,0.00044,0.0029,0.5,0.79,0.0087,2.3,3.3,0.042,5.8e-07,6.4e-07,5.2e-06,0.028,0.026,0.0005,4.2e-05,9.8e-06,0.00033,1.4e-05,2.7e-05,0.00033,0,0 -36990000,0.66,-0.00025,0.021,0.75,-4.1,-3.5,-0.042,-1e+06,1.2e+04,-3.7e+02,-0.00072,-0.0058,-0.00016,-0.018,0.051,-0.097,-0.2,-0.042,0.47,3.7e-05,-0.0033,-0.032,0,0,0.00026,0.00044,0.0029,0.53,0.83,0.0086,2.5,3.6,0.042,5.9e-07,6.4e-07,5.1e-06,0.028,0.026,0.0005,4.2e-05,9.7e-06,0.00033,1.4e-05,2.7e-05,0.00033,0,0 -37090000,0.66,-0.00027,0.021,0.75,-4.2,-3.6,-0.035,-1e+06,1.2e+04,-3.7e+02,-0.00072,-0.0058,-0.00016,-0.018,0.051,-0.097,-0.2,-0.042,0.47,5.5e-05,-0.0033,-0.032,0,0,0.00026,0.00044,0.0029,0.56,0.87,0.0086,2.7,3.9,0.041,5.9e-07,6.4e-07,5.1e-06,0.028,0.026,0.0005,4.2e-05,9.7e-06,0.00033,1.4e-05,2.7e-05,0.00033,0,0 -37190000,0.66,-0.0003,0.021,0.75,-4.2,-3.6,-0.028,-1e+06,1.2e+04,-3.7e+02,-0.00072,-0.0058,-0.00016,-0.018,0.051,-0.097,-0.2,-0.042,0.47,6.2e-05,-0.0033,-0.032,0,0,0.00026,0.00044,0.0029,0.59,0.91,0.0085,3,4.3,0.041,5.9e-07,6.4e-07,5.1e-06,0.028,0.026,0.0005,4.2e-05,9.6e-06,0.00033,1.4e-05,2.7e-05,0.00033,0,0 -37290000,0.66,-0.00038,0.021,0.75,-4.3,-3.7,-0.022,-1e+06,1.2e+04,-3.7e+02,-0.00072,-0.0058,-0.00017,-0.018,0.051,-0.097,-0.2,-0.042,0.47,6.9e-05,-0.0033,-0.032,0,0,0.00026,0.00043,0.0028,0.62,0.95,0.0086,3.2,4.6,0.04,5.9e-07,6.4e-07,5.1e-06,0.028,0.026,0.0005,4.1e-05,9.6e-06,0.00033,1.4e-05,2.6e-05,0.00033,0,0 -37390000,0.66,-0.00036,0.021,0.76,-4.3,-3.7,-0.016,-1e+06,1.2e+04,-3.7e+02,-0.00072,-0.0058,-0.00018,-0.018,0.051,-0.097,-0.2,-0.042,0.47,8.4e-05,-0.0033,-0.032,0,0,0.00026,0.00043,0.0028,0.65,1,0.0086,3.5,5,0.04,5.9e-07,6.4e-07,5.1e-06,0.028,0.026,0.0005,4.1e-05,9.5e-06,0.00033,1.4e-05,2.6e-05,0.00033,0,0 -37490000,0.65,-0.00039,0.022,0.76,-4.3,-3.8,-0.0099,-1e+06,1.2e+04,-3.7e+02,-0.00073,-0.0058,-0.0002,-0.018,0.051,-0.097,-0.2,-0.042,0.47,0.00011,-0.0032,-0.032,0,0,0.00026,0.00043,0.0028,0.68,1,0.0086,3.7,5.4,0.039,5.9e-07,6.4e-07,5.1e-06,0.028,0.026,0.0005,4.1e-05,9.5e-06,0.00033,1.4e-05,2.6e-05,0.00033,0,0 -37590000,0.65,-0.00042,0.022,0.76,-4.4,-3.9,-0.0027,-1e+06,1.2e+04,-3.7e+02,-0.00073,-0.0058,-0.00021,-0.018,0.051,-0.097,-0.2,-0.042,0.47,0.00012,-0.0032,-0.032,0,0,0.00025,0.00043,0.0028,0.71,1.1,0.0086,4,5.9,0.039,5.9e-07,6.4e-07,5.1e-06,0.028,0.026,0.0005,4.1e-05,9.4e-06,0.00033,1.4e-05,2.6e-05,0.00033,0,0 -37690000,0.65,-0.00052,0.022,0.76,-4.4,-3.9,0.0057,-1e+06,1.2e+04,-3.7e+02,-0.00073,-0.0058,-0.00023,-0.018,0.051,-0.097,-0.2,-0.042,0.47,0.00012,-0.0033,-0.032,0,0,0.00025,0.00043,0.0028,0.75,1.1,0.0087,4.3,6.4,0.039,5.9e-07,6.4e-07,5e-06,0.028,0.026,0.0005,4.1e-05,9.4e-06,0.00033,1.4e-05,2.6e-05,0.00033,0,0 -37790000,0.65,-0.0006,0.022,0.76,-4.5,-4,0.013,-1e+06,1.2e+04,-3.7e+02,-0.00073,-0.0058,-0.00023,-0.017,0.05,-0.097,-0.2,-0.042,0.47,0.00013,-0.0032,-0.032,0,0,0.00025,0.00042,0.0028,0.78,1.2,0.0087,4.7,6.9,0.039,5.9e-07,6.4e-07,5e-06,0.028,0.026,0.0005,4.1e-05,9.3e-06,0.00033,1.4e-05,2.6e-05,0.00033,0,0 -37890000,0.65,-0.00064,0.022,0.76,-4.5,-4.1,0.019,-1e+06,1.2e+04,-3.7e+02,-0.00073,-0.0058,-0.00024,-0.017,0.05,-0.097,-0.2,-0.042,0.47,0.00013,-0.0032,-0.032,0,0,0.00025,0.00042,0.0028,0.82,1.2,0.0088,5,7.4,0.038,5.9e-07,6.4e-07,5e-06,0.028,0.026,0.0005,4.1e-05,9.3e-06,0.00033,1.4e-05,2.6e-05,0.00033,0,0 -37990000,0.65,-0.00071,0.022,0.76,-4.5,-4.1,0.027,-1e+06,1.2e+04,-3.7e+02,-0.00073,-0.0058,-0.00024,-0.017,0.05,-0.097,-0.2,-0.042,0.47,0.00013,-0.0032,-0.032,0,0,0.00025,0.00042,0.0028,0.86,1.3,0.0089,5.4,8,0.038,5.9e-07,6.4e-07,5e-06,0.028,0.026,0.0005,4.1e-05,9.2e-06,0.00033,1.4e-05,2.6e-05,0.00033,0,0 -38090000,0.65,-0.00077,0.022,0.76,-4.6,-4.2,0.037,-1e+06,1.2e+04,-3.7e+02,-0.00073,-0.0058,-0.00026,-0.017,0.05,-0.098,-0.2,-0.042,0.47,0.00015,-0.0032,-0.032,0,0,0.00025,0.00042,0.0028,0.9,1.3,0.0089,5.8,8.6,0.038,5.9e-07,6.4e-07,5e-06,0.028,0.026,0.0005,4e-05,9.2e-06,0.00033,1.4e-05,2.6e-05,0.00033,0,0 -38190000,0.65,-0.00078,0.022,0.76,-4.6,-4.2,0.043,-1e+06,1.2e+04,-3.7e+02,-0.00073,-0.0058,-0.00026,-0.017,0.05,-0.098,-0.2,-0.042,0.47,0.00015,-0.0032,-0.032,0,0,0.00025,0.00042,0.0028,0.94,1.4,0.009,6.3,9.2,0.038,5.9e-07,6.4e-07,4.9e-06,0.028,0.026,0.0005,4e-05,9.1e-06,0.00033,1.4e-05,2.6e-05,0.00033,0,0 -38290000,0.65,-0.00081,0.022,0.76,-4.6,-4.3,0.05,-1e+06,1.2e+04,-3.7e+02,-0.00073,-0.0058,-0.00026,-0.017,0.05,-0.098,-0.2,-0.042,0.47,0.00015,-0.0032,-0.032,0,0,0.00025,0.00041,0.0027,0.98,1.4,0.0091,6.7,9.9,0.038,5.9e-07,6.4e-07,4.9e-06,0.028,0.026,0.0005,4e-05,9.1e-06,0.00033,1.4e-05,2.6e-05,0.00033,0,0 -38390000,0.65,-0.0008,0.022,0.76,-4.7,-4.4,0.056,-1e+06,1.2e+04,-3.7e+02,-0.00074,-0.0058,-0.00026,-0.017,0.05,-0.098,-0.2,-0.042,0.47,0.00016,-0.0032,-0.032,0,0,0.00025,0.00041,0.0027,1,1.5,0.0092,7.2,11,0.038,5.9e-07,6.4e-07,4.9e-06,0.028,0.026,0.0005,4e-05,9e-06,0.00033,1.4e-05,2.6e-05,0.00033,0,0 -38490000,0.65,-0.00084,0.022,0.76,-4.7,-4.4,0.061,-1e+06,1.2e+04,-3.7e+02,-0.00074,-0.0058,-0.00027,-0.017,0.05,-0.098,-0.2,-0.042,0.47,0.00018,-0.0032,-0.032,0,0,0.00025,0.00041,0.0027,1.1,1.5,0.0093,7.7,11,0.038,5.8e-07,6.3e-07,4.9e-06,0.028,0.026,0.0005,4e-05,9e-06,0.00033,1.4e-05,2.5e-05,0.00033,0,0 -38590000,0.65,-0.00081,0.022,0.76,-4.8,-4.5,0.067,-1e+06,1.2e+04,-3.7e+02,-0.00075,-0.0058,-0.00027,-0.017,0.05,-0.098,-0.2,-0.042,0.47,0.00018,-0.0031,-0.032,0,0,0.00025,0.00041,0.0027,1.1,1.6,0.0094,8.2,12,0.038,5.8e-07,6.3e-07,4.9e-06,0.028,0.026,0.0005,4e-05,8.9e-06,0.00033,1.4e-05,2.5e-05,0.00033,0,0 -38690000,0.65,-0.00086,0.022,0.76,-4.8,-4.5,0.071,-1e+06,1.2e+04,-3.7e+02,-0.00075,-0.0058,-0.00028,-0.017,0.05,-0.098,-0.2,-0.042,0.47,0.0002,-0.0031,-0.032,0,0,0.00025,0.00041,0.0027,1.1,1.7,0.0095,8.8,13,0.038,5.8e-07,6.3e-07,4.8e-06,0.028,0.026,0.0005,4e-05,8.9e-06,0.00033,1.4e-05,2.5e-05,0.00033,0,0 -38790000,0.65,-0.00087,0.022,0.76,-4.8,-4.6,0.077,-1e+06,1.2e+04,-3.7e+02,-0.00076,-0.0058,-0.00029,-0.017,0.05,-0.098,-0.2,-0.042,0.47,0.0002,-0.0031,-0.032,0,0,0.00025,0.00041,0.0027,1.2,1.7,0.0096,9.4,14,0.038,5.8e-07,6.3e-07,4.8e-06,0.028,0.026,0.0005,4e-05,8.8e-06,0.00033,1.4e-05,2.5e-05,0.00033,0,0 -38890000,0.65,-0.00099,0.022,0.76,-4.8,-4.6,0.58,-1e+06,1.2e+04,-3.7e+02,-0.00075,-0.0058,-0.00029,-0.017,0.05,-0.098,-0.2,-0.042,0.47,0.00019,-0.0031,-0.032,0,0,0.00025,0.0004,0.0027,1.2,1.7,0.0096,10,15,0.038,5.8e-07,6.3e-07,4.8e-06,0.028,0.026,0.0005,4e-05,8.8e-06,0.00033,1.4e-05,2.5e-05,0.00033,0,0 +10000,1,-0.0094,-0.01,-3.2e-06,0.00023,7.3e-05,-0.011,7.2e-06,2.2e-06,-0.00045,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0.01,0.00018,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +90000,1,-0.0094,-0.011,6.9e-05,-0.00047,0.0026,-0.026,-1.6e-05,0.00011,-0.0023,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0.01,0.00046,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +190000,1,-0.0094,-0.011,2.8e-05,6.9e-05,0.004,-0.041,-5.9e-06,0.00043,-0.0044,-3e-11,-2.7e-12,5.6e-13,0,0,-5e-08,0,0,0,0,0,0,0,0,0.011,0.011,0.00093,25,25,10,1e+02,1e+02,1,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +290000,1,-0.0094,-0.011,6.3e-05,0.001,0.0064,-0.053,3.8e-05,0.00036,-0.007,9.1e-10,1e-10,-1.7e-11,0,0,-2.5e-06,0,0,0,0,0,0,0,0,0.012,0.012,0.00077,25,25,9.6,0.37,0.37,0.41,0.01,0.01,0.0052,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +390000,1,-0.0094,-0.011,7e-05,0.0024,0.0083,-0.059,0.00021,0.0011,-0.0094,-1.1e-08,2.8e-09,2.9e-10,0,0,-1.5e-05,0,0,0,0,0,0,0,0,0.012,0.012,0.0012,25,25,8.1,0.97,0.97,0.32,0.01,0.01,0.0052,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +490000,1,-0.0095,-0.012,2e-05,0.0039,0.0048,-0.06,0.00024,0.00049,-0.011,1.6e-06,-3.7e-07,-4.2e-08,0,0,-4.1e-05,0,0,0,0,0,0,0,0,0.013,0.013,0.00073,7.8,7.8,5.9,0.34,0.34,0.31,0.01,0.01,0.0024,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +590000,1,-0.0095,-0.012,2.6e-05,0.006,0.0073,-0.059,0.00074,0.0011,-0.012,1.6e-06,-3.4e-07,-4e-08,0,0,-7.3e-05,0,0,0,0,0,0,0,0,0.015,0.015,0.001,7.9,7.9,4.2,0.67,0.67,0.32,0.01,0.01,0.0024,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +690000,1,-0.0096,-0.012,8.6e-05,0.0063,0.0052,-0.059,0.0005,0.00055,-0.013,5.5e-06,-3.2e-06,-1.8e-07,0,0,-0.00012,0,0,0,0,0,0,0,0,0.016,0.016,0.00063,2.7,2.7,2.8,0.26,0.26,0.29,0.01,0.01,0.0012,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +790000,1,-0.0097,-0.013,9.9e-05,0.0086,0.0073,-0.063,0.0012,0.0012,-0.014,5.4e-06,-3.1e-06,-1.8e-07,0,0,-0.00016,0,0,0,0,0,0,0,0,0.018,0.018,0.0008,2.8,2.8,2,0.42,0.42,0.28,0.01,0.01,0.0012,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +890000,1,-0.0098,-0.013,0.00012,0.01,0.006,-0.077,0.00096,0.00072,-0.021,1.6e-05,-1.5e-05,-6.5e-07,0,0,-0.00016,0,0,0,0,0,0,0,0,0.019,0.019,0.00053,1.3,1.3,2,0.2,0.2,0.43,0.0099,0.01,0.00068,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +990000,1,-0.0099,-0.013,0.00013,0.015,0.0064,-0.092,0.0022,0.0014,-0.029,1.6e-05,-1.5e-05,-6.5e-07,0,0,-0.00016,0,0,0,0,0,0,0,0,0.021,0.021,0.00065,1.5,1.5,2,0.3,0.3,0.61,0.0099,0.01,0.00068,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +1090000,1,-0.01,-0.014,0.00013,0.016,0.0051,-0.11,0.0018,0.00086,-0.039,4.1e-05,-6.2e-05,-2.1e-06,0,0,-0.00016,0,0,0,0,0,0,0,0,0.023,0.023,0.00047,0.93,0.93,2,0.17,0.17,0.84,0.0098,0.0098,0.00042,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +1190000,1,-0.01,-0.014,0.0001,0.02,0.0053,-0.12,0.0035,0.0014,-0.051,4.1e-05,-6.2e-05,-2.1e-06,0,0,-0.00016,0,0,0,0,0,0,0,0,0.025,0.025,0.00055,1.1,1.1,2,0.24,0.24,1.1,0.0098,0.0098,0.00042,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +1290000,1,-0.01,-0.014,0.00016,0.02,0.0044,-0.14,0.0027,0.00089,-0.064,8.5e-05,-0.00019,-5.6e-06,0,0,-0.00016,0,0,0,0,0,0,0,0,0.026,0.026,0.00042,0.89,0.89,2,0.15,0.15,1.4,0.0095,0.0095,0.00029,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +1390000,1,-0.01,-0.014,0.00017,0.026,0.0042,-0.15,0.0051,0.0013,-0.078,8.5e-05,-0.00019,-5.6e-06,0,0,-0.00016,0,0,0,0,0,0,0,0,0.028,0.028,0.00048,1.2,1.2,2,0.21,0.21,1.7,0.0095,0.0095,0.00029,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +1490000,1,-0.01,-0.014,0.00015,0.024,0.0029,-0.16,0.0038,0.00083,-0.093,0.00015,-0.00045,-1.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.027,0.027,0.00038,0.96,0.96,2,0.14,0.14,2.1,0.0088,0.0088,0.0002,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +1590000,1,-0.01,-0.014,0.00015,0.03,0.0035,-0.18,0.0065,0.0012,-0.11,0.00015,-0.00045,-1.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.03,0.03,0.00043,1.3,1.3,2,0.2,0.2,2.6,0.0088,0.0088,0.0002,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +1690000,1,-0.011,-0.014,0.00012,0.028,-9.5e-05,-0.19,0.0045,0.00063,-0.13,0.0002,-0.00088,-2.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.026,0.026,0.00034,1,1,2,0.14,0.14,3,0.0078,0.0078,0.00015,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +1790000,1,-0.011,-0.014,9.5e-05,0.035,-0.0019,-0.2,0.0076,0.00054,-0.15,0.0002,-0.00088,-2.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.028,0.028,0.00038,1.3,1.3,2,0.2,0.2,3.5,0.0078,0.0078,0.00015,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +1890000,1,-0.011,-0.015,7.5e-05,0.043,-0.0032,-0.22,0.011,0.00029,-0.17,0.0002,-0.00088,-2.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.031,0.031,0.00043,1.7,1.7,2,0.31,0.31,4.1,0.0078,0.0078,0.00015,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +1990000,1,-0.011,-0.014,8.5e-05,0.036,-0.0046,-0.23,0.0082,-0.00027,-0.19,0.00022,-0.0014,-3.3e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.025,0.025,0.00034,1.3,1.3,2.1,0.2,0.2,4.7,0.0067,0.0067,0.00011,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +2090000,1,-0.011,-0.014,4.7e-05,0.041,-0.0071,-0.24,0.012,-0.00085,-0.22,0.00022,-0.0014,-3.3e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.027,0.027,0.00038,1.7,1.7,2.1,0.31,0.31,5.3,0.0067,0.0067,0.00011,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +2190000,1,-0.011,-0.014,5.8e-05,0.033,-0.0068,-0.26,0.0079,-0.00096,-0.24,0.00017,-0.002,-4.3e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.02,0.02,0.00031,1.2,1.2,2.1,0.2,0.2,6,0.0055,0.0055,8.9e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +2290000,1,-0.011,-0.014,4.5e-05,0.039,-0.0093,-0.27,0.011,-0.0017,-0.27,0.00017,-0.002,-4.3e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.022,0.022,0.00034,1.5,1.5,2.1,0.3,0.3,6.7,0.0055,0.0055,8.9e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +2390000,1,-0.011,-0.013,6.2e-05,0.03,-0.0087,-0.29,0.0074,-0.0015,-0.3,9e-05,-0.0025,-5.2e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,0.017,0.017,0.00028,1,1,2.1,0.19,0.19,7.4,0.0046,0.0046,7.1e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +2490000,1,-0.011,-0.013,4.4e-05,0.035,-0.011,-0.3,0.011,-0.0024,-0.32,9e-05,-0.0025,-5.2e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,0.018,0.018,0.00031,1.3,1.3,2.1,0.28,0.28,8.2,0.0046,0.0046,7.1e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +2590000,1,-0.011,-0.013,5.8e-05,0.026,-0.009,-0.31,0.0068,-0.0018,-0.36,-1.4e-05,-0.0029,-5.8e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,0.014,0.014,0.00026,0.89,0.89,2.1,0.18,0.18,9.1,0.0038,0.0038,5.8e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +2690000,1,-0.011,-0.013,5.5e-05,0.03,-0.01,-0.33,0.0097,-0.0028,-0.39,-1.4e-05,-0.0029,-5.8e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,0.015,0.015,0.00028,1.1,1.1,2.2,0.25,0.25,10,0.0038,0.0038,5.8e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +2790000,1,-0.011,-0.013,4.8e-05,0.023,-0.0093,-0.34,0.0062,-0.0019,-0.42,-0.00012,-0.0033,-6.3e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,0.011,0.011,0.00024,0.77,0.77,2.2,0.16,0.16,11,0.0032,0.0032,4.8e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +2890000,1,-0.011,-0.013,-1.8e-06,0.027,-0.011,-0.35,0.0087,-0.0029,-0.46,-0.00012,-0.0033,-6.3e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,0.013,0.013,0.00026,0.95,0.95,2.2,0.23,0.23,12,0.0032,0.0032,4.8e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +2990000,1,-0.011,-0.013,4.6e-05,0.022,-0.0095,-0.36,0.0057,-0.0021,-0.49,-0.00023,-0.0036,-6.7e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,0.0099,0.0099,0.00022,0.67,0.67,2.2,0.15,0.15,13,0.0027,0.0027,4e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +3090000,1,-0.011,-0.013,4.8e-05,0.025,-0.011,-0.38,0.008,-0.0031,-0.53,-0.00023,-0.0036,-6.7e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,0.011,0.011,0.00024,0.83,0.83,2.2,0.22,0.22,14,0.0027,0.0027,4e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +3190000,1,-0.011,-0.013,-8.9e-06,0.02,-0.0086,-0.39,0.0053,-0.0021,-0.57,-0.00034,-0.0039,-7e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.0087,0.0087,0.00021,0.59,0.59,2.3,0.14,0.14,15,0.0023,0.0023,3.4e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +3290000,1,-0.011,-0.013,3.1e-05,0.023,-0.01,-0.4,0.0074,-0.003,-0.61,-0.00034,-0.0039,-7e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.0096,0.0096,0.00022,0.73,0.73,2.3,0.2,0.2,16,0.0023,0.0023,3.4e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +3390000,1,-0.011,-0.012,2.5e-06,0.018,-0.0091,-0.42,0.0049,-0.0021,-0.65,-0.00044,-0.0041,-7.3e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.0078,0.0078,0.00019,0.53,0.53,2.3,0.14,0.14,18,0.002,0.002,2.9e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +3490000,1,-0.011,-0.013,-5e-06,0.022,-0.012,-0.43,0.0069,-0.0031,-0.69,-0.00044,-0.0041,-7.3e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.0086,0.0086,0.00021,0.66,0.66,2.3,0.19,0.19,19,0.002,0.002,2.9e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +3590000,1,-0.011,-0.012,1.8e-05,0.017,-0.011,-0.44,0.0047,-0.0023,-0.73,-0.00055,-0.0044,-7.5e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.007,0.007,0.00018,0.49,0.49,2.4,0.13,0.13,20,0.0017,0.0017,2.5e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +3690000,1,-0.011,-0.012,0.00014,0.019,-0.014,-0.46,0.0065,-0.0035,-0.78,-0.00055,-0.0044,-7.5e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.0077,0.0077,0.00019,0.6,0.6,2.4,0.18,0.18,22,0.0017,0.0017,2.5e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +3790000,1,-0.011,-0.012,0.00019,0.016,-0.013,-0.47,0.0044,-0.0026,-0.82,-0.00067,-0.0046,-7.6e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.0063,0.0063,0.00017,0.45,0.45,2.4,0.12,0.12,23,0.0014,0.0014,2.1e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +3890000,1,-0.011,-0.012,0.00015,0.017,-0.014,-0.48,0.006,-0.0039,-0.87,-0.00067,-0.0046,-7.6e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.0069,0.0069,0.00018,0.55,0.55,2.4,0.17,0.17,24,0.0014,0.0014,2.1e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +3990000,1,-0.011,-0.012,0.00016,0.02,-0.016,-0.5,0.0079,-0.0055,-0.92,-0.00067,-0.0046,-7.6e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.0075,0.0075,0.00019,0.67,0.67,2.5,0.23,0.23,26,0.0014,0.0014,2.1e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +4090000,1,-0.011,-0.012,0.00015,0.017,-0.014,-0.51,0.0056,-0.0041,-0.97,-0.00079,-0.0047,-7.7e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.0062,0.0062,0.00017,0.51,0.51,2.5,0.16,0.16,28,0.0012,0.0012,1.9e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +4190000,1,-0.011,-0.012,0.00013,0.02,-0.016,-0.53,0.0075,-0.0056,-1,-0.00079,-0.0047,-7.7e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.0067,0.0067,0.00018,0.61,0.61,2.5,0.21,0.21,29,0.0012,0.0012,1.9e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +4290000,1,-0.01,-0.012,8.2e-05,0.017,-0.012,-0.54,0.0054,-0.0041,-1.1,-0.00091,-0.0049,-7.7e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.0056,0.0056,0.00016,0.47,0.47,2.6,0.15,0.15,31,0.00097,0.00097,1.6e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +4390000,1,-0.01,-0.012,0.0001,0.018,-0.013,-0.55,0.0071,-0.0053,-1.1,-0.00091,-0.0049,-7.7e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.006,0.006,0.00017,0.56,0.56,2.6,0.2,0.2,33,0.00097,0.00097,1.6e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +4490000,1,-0.01,-0.012,0.00016,0.014,-0.0097,-0.57,0.0051,-0.0037,-1.2,-0.001,-0.005,-7.8e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,0.0049,0.0049,0.00016,0.43,0.43,2.6,0.14,0.14,34,0.0008,0.0008,1.5e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +4590000,1,-0.01,-0.012,0.00019,0.017,-0.011,-0.58,0.0067,-0.0047,-1.2,-0.001,-0.005,-7.8e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,0.0053,0.0053,0.00017,0.52,0.52,2.7,0.19,0.19,36,0.0008,0.0008,1.5e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +4690000,1,-0.01,-0.012,0.0002,0.014,-0.0096,-0.6,0.0048,-0.0033,-1.3,-0.0011,-0.0052,-7.8e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,0.0044,0.0044,0.00015,0.4,0.4,2.7,0.14,0.14,38,0.00065,0.00065,1.3e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +4790000,1,-0.01,-0.012,0.00019,0.015,-0.011,-0.61,0.0062,-0.0043,-1.4,-0.0011,-0.0052,-7.8e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,0.0047,0.0047,0.00016,0.48,0.48,2.7,0.18,0.18,40,0.00065,0.00065,1.3e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +4890000,1,-0.01,-0.011,0.00017,0.012,-0.0097,-0.63,0.0044,-0.0031,-1.4,-0.0012,-0.0053,-7.9e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,0.0039,0.0039,0.00014,0.37,0.37,2.8,0.13,0.13,42,0.00053,0.00053,1.1e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +4990000,1,-0.01,-0.012,0.00015,0.015,-0.01,-0.64,0.0058,-0.0041,-1.5,-0.0012,-0.0053,-7.9e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,0.0041,0.0041,0.00015,0.44,0.44,2.8,0.17,0.17,44,0.00053,0.00053,1.1e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +5090000,1,-0.01,-0.011,0.0002,0.011,-0.0081,-0.66,0.0041,-0.003,-1.6,-0.0013,-0.0054,-7.9e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,0.0034,0.0034,0.00014,0.34,0.34,2.8,0.12,0.12,47,0.00043,0.00043,1e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +5190000,1,-0.01,-0.011,0.00022,0.013,-0.0095,-0.67,0.0053,-0.0039,-1.6,-0.0013,-0.0054,-7.9e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,0.0036,0.0036,0.00014,0.4,0.4,2.9,0.16,0.16,49,0.00043,0.00043,1e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +5290000,1,-0.0099,-0.011,0.00021,0.0086,-0.007,-0.68,0.0037,-0.0027,-1.7,-0.0013,-0.0055,-7.9e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,0.003,0.003,0.00013,0.31,0.31,2.9,0.12,0.12,51,0.00034,0.00034,9.3e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +5390000,1,-0.0099,-0.011,0.00027,0.0081,-0.0078,-0.7,0.0045,-0.0035,-1.8,-0.0013,-0.0055,-7.9e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,0.0032,0.0032,0.00014,0.36,0.36,3,0.16,0.16,54,0.00034,0.00034,9.3e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +5490000,1,-0.0098,-0.011,0.00028,0.0055,-0.0059,-0.71,0.0031,-0.0025,-1.8,-0.0014,-0.0055,-7.9e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,0.0026,0.0026,0.00013,0.28,0.28,3,0.11,0.11,56,0.00028,0.00028,8.4e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +5590000,1,-0.0097,-0.011,0.00026,0.0061,-0.0063,-0.73,0.0036,-0.003,-1.9,-0.0014,-0.0055,-7.9e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,0.0028,0.0028,0.00013,0.33,0.33,3,0.15,0.15,59,0.00028,0.00028,8.4e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +5690000,1,-0.0096,-0.011,0.00034,0.0041,-0.0036,-0.74,0.0025,-0.0021,-2,-0.0014,-0.0056,-7.9e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,0.0023,0.0023,0.00012,0.26,0.26,3.1,0.11,0.11,61,0.00022,0.00022,7.6e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +5790000,1,-0.0095,-0.011,0.00033,0.0044,-0.0026,-0.75,0.0029,-0.0024,-2,-0.0014,-0.0056,-7.9e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,0.0025,0.0025,0.00013,0.3,0.3,3.1,0.14,0.14,64,0.00022,0.00022,7.6e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +5890000,1,-0.0095,-0.011,0.00031,0.0038,-0.00081,0.0028,0.002,-0.0016,-3.7e+02,-0.0014,-0.0056,-7.9e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,0.0021,0.0021,0.00012,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.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +5990000,1,-0.0094,-0.011,0.00033,0.0041,0.00065,0.015,0.0023,-0.0015,-3.7e+02,-0.0014,-0.0056,-7.9e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,0.0022,0.0022,0.00012,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.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +6090000,1,-0.0094,-0.011,0.00032,0.0051,0.0018,-0.011,0.0028,-0.0014,-3.7e+02,-0.0014,-0.0056,-7.9e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,0.0023,0.0023,0.00013,0.31,0.31,7,0.17,0.17,0.33,0.00018,0.00018,6.9e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +6190000,1,-0.0094,-0.011,0.00024,0.0038,0.0042,-0.005,0.002,-0.00048,-3.7e+02,-0.0015,-0.0056,-7.9e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,0.0019,0.0019,0.00012,0.25,0.25,4.9,0.13,0.13,0.32,0.00015,0.00015,6.3e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +6290000,1,-0.0094,-0.011,0.00022,0.005,0.0042,-0.012,0.0025,-6.7e-05,-3.7e+02,-0.0015,-0.0056,-7.9e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.002,0.002,0.00012,0.28,0.28,3.2,0.16,0.16,0.3,0.00015,0.00015,6.3e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +6390000,1,-0.0093,-0.011,0.00024,0.0043,0.0052,-0.05,0.0019,0.0004,-3.7e+02,-0.0015,-0.0057,-8e-05,0,0,-0.0001,0,0,0,0,0,0,0,0,0.0017,0.0017,0.00011,0.22,0.22,2.3,0.12,0.12,0.29,0.00012,0.00012,5.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +6490000,1,-0.0093,-0.011,0.00023,0.0049,0.0053,-0.052,0.0023,0.00093,-3.7e+02,-0.0015,-0.0057,-8e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,0.0018,0.0018,0.00012,0.26,0.26,1.5,0.15,0.15,0.26,0.00012,0.00012,5.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +6590000,1,-0.0094,-0.011,0.00017,0.0037,0.0053,-0.099,0.0018,0.00099,-3.7e+02,-0.0015,-0.0057,-8e-05,0,0,2.9e-05,0,0,0,0,0,0,0,0,0.0015,0.0015,0.00011,0.2,0.2,1.1,0.12,0.12,0.23,9.7e-05,9.7e-05,5.3e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +6690000,1,-0.0093,-0.011,9.6e-05,0.0046,0.0047,-0.076,0.0022,0.0015,-3.7e+02,-0.0015,-0.0057,-8e-05,0,0,-0.00029,0,0,0,0,0,0,0,0,0.0016,0.0016,0.00011,0.23,0.23,0.78,0.14,0.14,0.21,9.7e-05,9.7e-05,5.3e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +6790000,0.78,-0.024,0.0049,-0.63,-0.0056,0.0015,-0.11,0.0015,0.00055,-3.7e+02,-0.0014,-0.0057,-8.2e-05,0,0,-5.8e-05,-0.092,-0.02,0.51,-0.00081,-0.075,-0.061,0,0,0.0013,0.0013,0.072,0.18,0.18,0.6,0.11,0.11,0.2,8e-05,8e-05,4.9e-06,0.04,0.04,0.04,0.0014,0.0005,0.0014,0.0011,0.0015,0.0014,1,1 +6890000,0.78,-0.025,0.0057,-0.63,-0.03,-0.0077,-0.12,-9e-05,-0.0018,-3.7e+02,-0.0013,-0.0057,-8.3e-05,0,0,-0.00011,-0.1,-0.022,0.51,-0.0011,-0.083,-0.067,0,0,0.0013,0.0013,0.057,0.18,0.18,0.46,0.14,0.14,0.18,8e-05,8e-05,4.9e-06,0.04,0.04,0.04,0.0013,0.00024,0.0013,0.00094,0.0014,0.0013,1,1 +6990000,0.78,-0.025,0.0058,-0.63,-0.059,-0.018,-0.12,-0.0053,-0.0061,-3.7e+02,-0.0012,-0.0057,-8.7e-05,-2.7e-05,-6.9e-05,-0.00041,-0.1,-0.022,0.5,-0.0015,-0.084,-0.067,0,0,0.0013,0.0013,0.053,0.19,0.18,0.36,0.17,0.17,0.16,8e-05,8e-05,4.9e-06,0.04,0.04,0.04,0.0013,0.00018,0.0013,0.0009,0.0014,0.0013,1,1 +7090000,0.78,-0.025,0.0059,-0.63,-0.087,-0.028,-0.12,-0.014,-0.012,-3.7e+02,-0.0011,-0.0057,-9.1e-05,-8.3e-05,-0.00024,-0.00077,-0.1,-0.023,0.5,-0.0017,-0.085,-0.068,0,0,0.0013,0.0013,0.052,0.19,0.19,0.29,0.2,0.2,0.16,7.9e-05,7.9e-05,4.9e-06,0.04,0.04,0.04,0.0013,0.00015,0.0013,0.00089,0.0013,0.0013,1,1 +7190000,0.78,-0.025,0.006,-0.63,-0.11,-0.037,-0.15,-0.024,-0.018,-3.7e+02,-0.001,-0.0057,-9.3e-05,-7.7e-05,-0.00036,-0.00056,-0.1,-0.023,0.5,-0.0017,-0.085,-0.068,0,0,0.0013,0.0013,0.051,0.21,0.21,0.24,0.23,0.23,0.15,7.9e-05,7.9e-05,4.9e-06,0.04,0.04,0.04,0.0013,0.00013,0.0013,0.00088,0.0013,0.0013,1,1 +7290000,0.78,-0.025,0.0059,-0.63,-0.14,-0.04,-0.14,-0.035,-0.019,-3.7e+02,-0.0011,-0.0057,-9e-05,-4.6e-05,-0.00024,-0.0012,-0.1,-0.023,0.5,-0.0017,-0.085,-0.068,0,0,0.0013,0.0013,0.05,0.22,0.22,0.2,0.28,0.28,0.14,7.9e-05,7.9e-05,4.9e-06,0.04,0.04,0.04,0.0013,0.00012,0.0013,0.00087,0.0013,0.0013,1,1 +7390000,0.78,-0.025,0.0059,-0.63,-0.16,-0.048,-0.16,-0.049,-0.025,-3.7e+02,-0.0011,-0.0057,-9e-05,8.1e-07,-0.00028,-0.0014,-0.1,-0.023,0.5,-0.0017,-0.085,-0.068,0,0,0.0013,0.0013,0.05,0.24,0.24,0.18,0.32,0.32,0.13,7.8e-05,7.9e-05,4.9e-06,0.04,0.04,0.039,0.0013,0.00011,0.0013,0.00087,0.0013,0.0013,1,1 +7490000,0.78,-0.025,0.0059,-0.63,-0.19,-0.061,-0.16,-0.066,-0.038,-3.7e+02,-0.00093,-0.0057,-9.3e-05,1.6e-05,-0.0005,-0.0022,-0.1,-0.023,0.5,-0.0017,-0.085,-0.069,0,0,0.0013,0.0013,0.049,0.27,0.26,0.15,0.37,0.37,0.12,7.8e-05,7.8e-05,4.9e-06,0.04,0.04,0.039,0.0013,0.0001,0.0013,0.00086,0.0013,0.0013,1,1 +7590000,0.78,-0.025,0.0059,-0.63,-0.21,-0.067,-0.16,-0.082,-0.044,-3.7e+02,-0.00093,-0.0056,-9e-05,0.00011,-0.00047,-0.003,-0.1,-0.023,0.5,-0.0016,-0.085,-0.068,0,0,0.0013,0.0013,0.049,0.29,0.29,0.14,0.43,0.43,0.12,7.7e-05,7.7e-05,4.9e-06,0.04,0.04,0.039,0.0013,0.0001,0.0013,0.00086,0.0013,0.0013,1,1 +7690000,0.78,-0.025,0.0059,-0.63,-0.24,-0.079,-0.16,-0.1,-0.058,-3.7e+02,-0.00082,-0.0056,-9.3e-05,8.3e-05,-0.00057,-0.005,-0.1,-0.023,0.5,-0.0016,-0.086,-0.069,0,0,0.0014,0.0014,0.049,0.32,0.32,0.13,0.49,0.49,0.11,7.6e-05,7.7e-05,4.9e-06,0.04,0.04,0.039,0.0013,9.6e-05,0.0013,0.00086,0.0013,0.0013,1,1 +7790000,0.78,-0.025,0.006,-0.63,-0.27,-0.088,-0.16,-0.14,-0.072,-3.7e+02,-0.00074,-0.0057,-9.9e-05,-0.00013,-0.00063,-0.007,-0.1,-0.023,0.5,-0.0016,-0.086,-0.069,0,0,0.0014,0.0014,0.049,0.35,0.35,0.12,0.56,0.55,0.11,7.5e-05,7.5e-05,4.9e-06,0.04,0.04,0.038,0.0013,9.3e-05,0.0013,0.00086,0.0013,0.0013,1,1 +7890000,0.78,-0.025,0.006,-0.63,-0.29,-0.099,-0.15,-0.16,-0.086,-3.7e+02,-0.00067,-0.0056,-9.8e-05,-8.1e-05,-0.00064,-0.0095,-0.1,-0.023,0.5,-0.0016,-0.086,-0.069,0,0,0.0014,0.0014,0.049,0.39,0.38,0.11,0.63,0.63,0.1,7.4e-05,7.4e-05,4.9e-06,0.04,0.04,0.038,0.0013,9e-05,0.0013,0.00086,0.0013,0.0013,1,1 +7990000,0.78,-0.025,0.006,-0.63,-0.32,-0.11,-0.16,-0.19,-0.098,-3.7e+02,-0.00064,-0.0056,-9.7e-05,2.6e-06,-0.00064,-0.011,-0.1,-0.023,0.5,-0.0016,-0.086,-0.069,0,0,0.0014,0.0014,0.049,0.43,0.42,0.1,0.71,0.71,0.099,7.2e-05,7.3e-05,4.9e-06,0.04,0.04,0.038,0.0013,8.8e-05,0.0013,0.00086,0.0013,0.0013,1,1 +8090000,0.78,-0.025,0.006,-0.63,-0.34,-0.12,-0.17,-0.21,-0.11,-3.7e+02,-0.00058,-0.0054,-9.5e-05,0.00017,-0.00074,-0.011,-0.1,-0.023,0.5,-0.0016,-0.086,-0.069,0,0,0.0014,0.0014,0.048,0.47,0.46,0.1,0.8,0.8,0.097,7e-05,7.1e-05,4.9e-06,0.04,0.04,0.037,0.0013,8.6e-05,0.0013,0.00086,0.0013,0.0013,1,1 +8190000,0.78,-0.025,0.0061,-0.63,-0.37,-0.13,-0.17,-0.25,-0.13,-3.7e+02,-0.00056,-0.0055,-9.8e-05,-2.7e-05,-0.00068,-0.013,-0.1,-0.023,0.5,-0.0016,-0.086,-0.069,0,0,0.0014,0.0014,0.048,0.52,0.51,0.099,0.9,0.89,0.094,6.8e-05,6.9e-05,4.9e-06,0.04,0.04,0.037,0.0013,8.5e-05,0.0013,0.00085,0.0013,0.0013,1,1 +8290000,0.78,-0.025,0.0061,-0.63,-0.019,-0.0046,-0.17,-0.27,-0.13,-3.7e+02,-0.00042,-0.0056,-0.0001,-6.1e-05,-0.00064,-0.017,-0.1,-0.023,0.5,-0.0016,-0.086,-0.069,0,0,0.0014,0.0015,0.048,25,25,0.097,1e+02,1e+02,0.091,6.6e-05,6.7e-05,4.9e-06,0.04,0.04,0.036,0.0013,8.3e-05,0.0013,0.00085,0.0013,0.0013,1,1 +8390000,0.78,-0.025,0.0061,-0.63,-0.046,-0.012,-0.17,-0.27,-0.13,-3.7e+02,-0.00038,-0.0055,-0.0001,-6.1e-05,-0.00064,-0.021,-0.1,-0.023,0.5,-0.0016,-0.086,-0.069,0,0,0.0015,0.0015,0.048,25,25,0.097,1e+02,1e+02,0.091,6.4e-05,6.5e-05,4.9e-06,0.04,0.04,0.035,0.0013,8.2e-05,0.0013,0.00085,0.0013,0.0013,1,1 +8490000,0.78,-0.026,0.0062,-0.63,-0.073,-0.02,-0.17,-0.27,-0.13,-3.7e+02,-0.00033,-0.0055,-0.0001,-6.1e-05,-0.00064,-0.025,-0.1,-0.023,0.5,-0.0016,-0.086,-0.069,0,0,0.0015,0.0015,0.048,25,25,0.096,51,51,0.089,6.2e-05,6.3e-05,4.9e-06,0.04,0.04,0.034,0.0013,8.1e-05,0.0013,0.00085,0.0013,0.0013,1,1 +8590000,0.78,-0.025,0.0062,-0.63,-0.099,-0.028,-0.16,-0.28,-0.14,-3.7e+02,-0.00051,-0.0056,-0.0001,-6.1e-05,-0.00064,-0.029,-0.1,-0.023,0.5,-0.0016,-0.086,-0.069,0,0,0.0015,0.0015,0.048,25,25,0.095,52,52,0.088,5.9e-05,6.1e-05,4.9e-06,0.04,0.04,0.034,0.0013,8e-05,0.0013,0.00085,0.0013,0.0013,1,1 +8690000,0.78,-0.025,0.0061,-0.63,-0.12,-0.036,-0.16,-0.28,-0.14,-3.7e+02,-0.00047,-0.0055,-9.9e-05,-6.1e-05,-0.00064,-0.034,-0.1,-0.023,0.5,-0.0016,-0.086,-0.069,0,0,0.0015,0.0015,0.048,24,24,0.096,35,35,0.088,5.7e-05,5.9e-05,4.9e-06,0.04,0.04,0.033,0.0013,7.9e-05,0.0013,0.00085,0.0013,0.0013,1,1 +8790000,0.78,-0.026,0.0062,-0.63,-0.15,-0.044,-0.15,-0.3,-0.14,-3.7e+02,-0.0004,-0.0055,-0.0001,-6.1e-05,-0.00064,-0.04,-0.1,-0.023,0.5,-0.0016,-0.086,-0.069,0,0,0.0015,0.0015,0.048,25,25,0.096,37,37,0.087,5.5e-05,5.6e-05,4.9e-06,0.04,0.04,0.032,0.0013,7.8e-05,0.0013,0.00085,0.0013,0.0013,1,1 +8890000,0.78,-0.026,0.0063,-0.63,-0.18,-0.05,-0.15,-0.3,-0.14,-3.7e+02,-0.00037,-0.0056,-0.00011,-6.1e-05,-0.00064,-0.044,-0.1,-0.023,0.5,-0.0016,-0.086,-0.069,0,0,0.0015,0.0016,0.048,24,24,0.095,28,28,0.086,5.2e-05,5.4e-05,4.9e-06,0.04,0.04,0.03,0.0013,7.7e-05,0.0013,0.00085,0.0013,0.0013,1,1 +8990000,0.78,-0.026,0.0064,-0.63,-0.21,-0.056,-0.14,-0.32,-0.15,-3.7e+02,-0.00029,-0.0057,-0.00011,-6.1e-05,-0.00064,-0.05,-0.1,-0.023,0.5,-0.0016,-0.086,-0.069,0,0,0.0015,0.0016,0.048,24,24,0.096,30,30,0.087,5e-05,5.1e-05,4.9e-06,0.04,0.04,0.029,0.0013,7.7e-05,0.0013,0.00085,0.0013,0.0013,1,1 +9090000,0.78,-0.026,0.0065,-0.63,-0.23,-0.06,-0.14,-0.33,-0.15,-3.7e+02,-0.00033,-0.0057,-0.00011,-6.1e-05,-0.00064,-0.052,-0.1,-0.023,0.5,-0.0016,-0.086,-0.069,0,0,0.0015,0.0016,0.048,23,23,0.095,25,25,0.086,4.7e-05,4.9e-05,4.9e-06,0.04,0.04,0.028,0.0013,7.6e-05,0.0013,0.00085,0.0013,0.0013,1,1 +9190000,0.78,-0.026,0.0063,-0.63,-0.25,-0.071,-0.14,-0.35,-0.16,-3.7e+02,-0.00029,-0.0055,-0.00011,-6.1e-05,-0.00064,-0.056,-0.1,-0.023,0.5,-0.0016,-0.086,-0.069,0,0,0.0015,0.0016,0.048,23,23,0.094,27,27,0.085,4.5e-05,4.6e-05,4.9e-06,0.04,0.04,0.027,0.0013,7.5e-05,0.0013,0.00085,0.0013,0.0013,1,1 +9290000,0.78,-0.026,0.0063,-0.63,-0.27,-0.077,-0.13,-0.35,-0.16,-3.7e+02,-0.00023,-0.0055,-0.00011,-6.1e-05,-0.00064,-0.06,-0.1,-0.023,0.5,-0.0016,-0.086,-0.069,0,0,0.0015,0.0016,0.048,21,21,0.093,23,23,0.085,4.2e-05,4.4e-05,4.9e-06,0.04,0.04,0.025,0.0013,7.5e-05,0.0013,0.00085,0.0013,0.0013,1,1 +9390000,0.78,-0.026,0.0065,-0.63,-0.3,-0.086,-0.13,-0.38,-0.17,-3.7e+02,-0.00018,-0.0055,-0.00011,-6.1e-05,-0.00064,-0.063,-0.1,-0.023,0.5,-0.0016,-0.086,-0.069,0,0,0.0016,0.0016,0.048,21,21,0.093,25,25,0.086,4e-05,4.2e-05,4.9e-06,0.04,0.04,0.024,0.0013,7.4e-05,0.0013,0.00085,0.0013,0.0013,1,1 +9490000,0.78,-0.026,0.0064,-0.63,-0.31,-0.092,-0.13,-0.38,-0.17,-3.7e+02,-0.00011,-0.0054,-0.0001,-6.1e-05,-0.00064,-0.067,-0.1,-0.023,0.5,-0.0016,-0.086,-0.069,0,0,0.0016,0.0016,0.048,19,19,0.091,22,22,0.085,3.8e-05,4e-05,4.9e-06,0.04,0.04,0.023,0.0013,7.4e-05,0.0013,0.00085,0.0013,0.0013,1,1 +9590000,0.78,-0.026,0.0063,-0.63,-0.34,-0.1,-0.12,-0.41,-0.18,-3.7e+02,-0.00024,-0.0053,-0.0001,-6.1e-05,-0.00064,-0.07,-0.1,-0.023,0.5,-0.0016,-0.086,-0.069,0,0,0.0016,0.0016,0.048,19,19,0.09,25,25,0.085,3.6e-05,3.7e-05,4.9e-06,0.04,0.04,0.022,0.0013,7.3e-05,0.0013,0.00085,0.0013,0.0013,1,1 +9690000,0.78,-0.026,0.0063,-0.63,-0.34,-0.1,-0.12,-0.41,-0.17,-3.7e+02,-0.0003,-0.0055,-0.0001,-6.1e-05,-0.00064,-0.075,-0.1,-0.023,0.5,-0.0016,-0.086,-0.069,0,0,0.0016,0.0016,0.048,17,17,0.089,22,22,0.086,3.4e-05,3.6e-05,4.9e-06,0.04,0.04,0.02,0.0013,7.3e-05,0.0013,0.00085,0.0013,0.0013,1,1 +9790000,0.78,-0.026,0.0063,-0.63,-0.38,-0.11,-0.1,-0.45,-0.19,-3.7e+02,-0.00026,-0.0054,-0.0001,-6.1e-05,-0.00064,-0.08,-0.1,-0.023,0.5,-0.0016,-0.086,-0.069,0,0,0.0016,0.0016,0.048,17,17,0.087,24,24,0.085,3.2e-05,3.4e-05,4.9e-06,0.04,0.04,0.019,0.0013,7.3e-05,0.0013,0.00085,0.0013,0.0013,1,1 +9890000,0.78,-0.026,0.0063,-0.63,-0.4,-0.12,-0.1,-0.48,-0.2,-3.7e+02,-0.00033,-0.0054,-0.0001,-6.1e-05,-0.00064,-0.083,-0.1,-0.023,0.5,-0.0016,-0.086,-0.069,0,0,0.0016,0.0016,0.048,17,17,0.084,28,28,0.085,3e-05,3.2e-05,4.9e-06,0.04,0.04,0.018,0.0013,7.2e-05,0.0013,0.00085,0.0013,0.0013,1,1 +9990000,0.78,-0.026,0.0063,-0.63,-0.4,-0.12,-0.096,-0.47,-0.19,-3.7e+02,-0.00035,-0.0055,-0.0001,-6.1e-05,-0.00064,-0.086,-0.1,-0.023,0.5,-0.0016,-0.086,-0.069,0,0,0.0016,0.0016,0.048,15,15,0.083,24,24,0.086,2.8e-05,3e-05,4.9e-06,0.04,0.04,0.017,0.0013,7.2e-05,0.0013,0.00085,0.0013,0.0013,1,1 +10090000,0.78,-0.026,0.0062,-0.63,-0.43,-0.12,-0.092,-0.51,-0.2,-3.7e+02,-0.00043,-0.0055,-0.0001,-6.1e-05,-0.00064,-0.089,-0.1,-0.023,0.5,-0.0016,-0.086,-0.069,0,0,0.0016,0.0016,0.048,15,15,0.081,27,27,0.085,2.7e-05,2.8e-05,4.9e-06,0.04,0.04,0.016,0.0013,7.2e-05,0.0013,0.00085,0.0013,0.0013,1,1 +10190000,0.78,-0.026,0.0065,-0.63,-0.43,-0.12,-0.092,-0.5,-0.2,-3.7e+02,-0.00044,-0.0057,-0.00011,-6.1e-05,-0.00064,-0.09,-0.1,-0.023,0.5,-0.0016,-0.086,-0.069,0,0,0.0016,0.0016,0.048,14,13,0.078,24,24,0.084,2.5e-05,2.7e-05,4.9e-06,0.04,0.04,0.015,0.0013,7.1e-05,0.0013,0.00085,0.0013,0.0013,1,1 +10290000,0.78,-0.026,0.0067,-0.63,-0.46,-0.12,-0.08,-0.55,-0.21,-3.7e+02,-0.00045,-0.0058,-0.00011,-6.1e-05,-0.00064,-0.096,-0.1,-0.023,0.5,-0.0016,-0.086,-0.069,0,0,0.0016,0.0016,0.048,14,14,0.076,27,27,0.085,2.4e-05,2.5e-05,4.9e-06,0.04,0.04,0.014,0.0013,7.1e-05,0.0013,0.00085,0.0013,0.0013,1,1 +10390000,0.78,-0.026,0.0068,-0.63,-0.017,-0.027,0.0097,-0.00031,-0.0021,-3.7e+02,-0.00042,-0.0058,-0.00011,-0.00013,-0.00061,-0.099,-0.1,-0.023,0.5,-0.0016,-0.086,-0.069,0,0,0.0016,0.0016,0.048,0.25,0.25,0.56,0.25,0.25,0.078,2.2e-05,2.4e-05,4.9e-06,0.04,0.04,0.013,0.0013,7.1e-05,0.0013,0.00085,0.0013,0.0013,1,1 +10490000,0.78,-0.026,0.0069,-0.63,-0.047,-0.034,0.023,-0.0035,-0.0051,-3.7e+02,-0.00042,-0.0058,-0.00011,-0.00035,-0.00043,-0.1,-0.1,-0.023,0.5,-0.0016,-0.086,-0.069,0,0,0.0016,0.0016,0.048,0.26,0.26,0.55,0.26,0.26,0.08,2.1e-05,2.3e-05,4.9e-06,0.04,0.04,0.012,0.0013,7.1e-05,0.0013,0.00085,0.0013,0.0013,1,1 +10590000,0.78,-0.026,0.0066,-0.63,-0.045,-0.023,0.026,0.0012,-0.0011,-3.7e+02,-0.00059,-0.0058,-0.00011,-3.5e-05,-0.0011,-0.1,-0.1,-0.023,0.5,-0.0015,-0.086,-0.069,0,0,0.0015,0.0016,0.048,0.13,0.13,0.27,0.13,0.13,0.073,2e-05,2.1e-05,4.9e-06,0.04,0.04,0.012,0.0013,7e-05,0.0013,0.00085,0.0013,0.0013,1,1 +10690000,0.78,-0.026,0.0066,-0.63,-0.073,-0.029,0.03,-0.0048,-0.0037,-3.7e+02,-0.00059,-0.0059,-0.00011,-9.8e-05,-0.0011,-0.1,-0.1,-0.023,0.5,-0.0015,-0.086,-0.069,0,0,0.0015,0.0016,0.048,0.14,0.14,0.26,0.14,0.14,0.078,1.9e-05,2e-05,4.9e-06,0.04,0.04,0.011,0.0013,7e-05,0.0013,0.00085,0.0013,0.0013,1,1 +10790000,0.78,-0.025,0.0064,-0.63,-0.069,-0.024,0.024,-0.0001,-0.0015,-3.7e+02,-0.00063,-0.0058,-0.00011,0.00028,-0.0037,-0.1,-0.1,-0.023,0.5,-0.0015,-0.086,-0.069,0,0,0.0015,0.0016,0.048,0.096,0.096,0.17,0.09,0.09,0.072,1.8e-05,1.9e-05,4.8e-06,0.04,0.039,0.011,0.0013,7e-05,0.0013,0.00084,0.0013,0.0013,1,1 +10890000,0.78,-0.025,0.0064,-0.63,-0.097,-0.03,0.02,-0.0084,-0.0042,-3.7e+02,-0.00063,-0.0058,-0.00011,0.0003,-0.0037,-0.1,-0.1,-0.023,0.5,-0.0015,-0.086,-0.069,0,0,0.0015,0.0015,0.048,0.11,0.11,0.16,0.096,0.096,0.075,1.7e-05,1.8e-05,4.8e-06,0.039,0.039,0.011,0.0013,6.9e-05,0.0013,0.00084,0.0013,0.0013,1,1 +10990000,0.78,-0.025,0.0059,-0.63,-0.085,-0.024,0.014,-0.0037,-0.0024,-3.7e+02,-0.00066,-0.0058,-0.0001,0.0011,-0.009,-0.11,-0.1,-0.023,0.5,-0.0014,-0.086,-0.069,0,0,0.0014,0.0015,0.048,0.086,0.086,0.12,0.099,0.099,0.071,1.6e-05,1.7e-05,4.8e-06,0.039,0.039,0.011,0.0013,6.9e-05,0.0013,0.00083,0.0013,0.0013,1,1 +11090000,0.78,-0.025,0.0057,-0.63,-0.11,-0.032,0.019,-0.013,-0.0054,-3.7e+02,-0.00069,-0.0057,-9.6e-05,0.0013,-0.0089,-0.11,-0.1,-0.023,0.5,-0.0014,-0.086,-0.069,0,0,0.0014,0.0015,0.048,0.1,0.1,0.11,0.11,0.11,0.074,1.5e-05,1.6e-05,4.8e-06,0.039,0.039,0.011,0.0013,6.9e-05,0.0013,0.00083,0.0013,0.0013,1,1 +11190000,0.78,-0.024,0.0051,-0.63,-0.095,-0.025,0.0078,-0.0051,-0.0021,-3.7e+02,-0.00079,-0.0057,-9.3e-05,0.0019,-0.016,-0.11,-0.1,-0.023,0.5,-0.0013,-0.087,-0.069,0,0,0.0013,0.0013,0.048,0.084,0.084,0.084,0.11,0.11,0.069,1.4e-05,1.5e-05,4.8e-06,0.038,0.038,0.011,0.0013,6.8e-05,0.0013,0.00082,0.0013,0.0013,1,1 +11290000,0.78,-0.024,0.0052,-0.63,-0.12,-0.029,0.0076,-0.016,-0.0046,-3.7e+02,-0.00074,-0.0058,-9.7e-05,0.0016,-0.016,-0.11,-0.1,-0.023,0.5,-0.0013,-0.087,-0.069,0,0,0.0013,0.0013,0.048,0.099,0.099,0.078,0.12,0.12,0.072,1.3e-05,1.4e-05,4.8e-06,0.038,0.038,0.01,0.0013,6.8e-05,0.0013,0.00082,0.0013,0.0013,1,1 +11390000,0.78,-0.022,0.0045,-0.63,-0.1,-0.024,0.002,-0.0027,-0.00099,-3.7e+02,-0.00084,-0.0059,-9.5e-05,0.0015,-0.022,-0.11,-0.11,-0.023,0.5,-0.0013,-0.088,-0.069,0,0,0.0011,0.0012,0.047,0.08,0.08,0.063,0.082,0.082,0.068,1.2e-05,1.3e-05,4.8e-06,0.037,0.037,0.01,0.0013,6.8e-05,0.0013,0.0008,0.0013,0.0013,1,1 +11490000,0.78,-0.022,0.0047,-0.63,-0.12,-0.026,0.0029,-0.014,-0.0031,-3.7e+02,-0.0008,-0.006,-0.0001,0.0009,-0.022,-0.11,-0.11,-0.023,0.5,-0.0014,-0.088,-0.069,0,0,0.0011,0.0012,0.047,0.095,0.095,0.058,0.089,0.089,0.069,1.2e-05,1.3e-05,4.8e-06,0.037,0.037,0.01,0.0013,6.8e-05,0.0013,0.0008,0.0013,0.0013,1,1 +11590000,0.78,-0.022,0.0041,-0.63,-0.1,-0.022,-0.003,-0.0044,-0.00098,-3.7e+02,-0.00085,-0.006,-9.8e-05,0.00079,-0.029,-0.11,-0.11,-0.023,0.5,-0.0015,-0.088,-0.069,0,0,0.001,0.001,0.047,0.078,0.078,0.049,0.068,0.068,0.066,1.1e-05,1.2e-05,4.8e-06,0.036,0.036,0.01,0.0012,6.7e-05,0.0013,0.00079,0.0013,0.0013,1,1 +11690000,0.78,-0.022,0.0042,-0.63,-0.12,-0.026,-0.0074,-0.016,-0.0033,-3.7e+02,-0.00079,-0.006,-0.0001,0.00062,-0.03,-0.11,-0.11,-0.023,0.5,-0.0015,-0.088,-0.069,0,0,0.001,0.001,0.047,0.093,0.093,0.046,0.075,0.075,0.066,1.1e-05,1.1e-05,4.8e-06,0.036,0.036,0.01,0.0012,6.7e-05,0.0013,0.00079,0.0013,0.0013,1,1 +11790000,0.78,-0.021,0.0037,-0.63,-0.1,-0.017,-0.0092,-0.0082,0.00018,-3.7e+02,-0.00082,-0.006,-9.7e-05,0.0012,-0.036,-0.11,-0.11,-0.023,0.5,-0.0014,-0.089,-0.069,0,0,0.00089,0.00092,0.047,0.077,0.076,0.039,0.06,0.06,0.063,1e-05,1.1e-05,4.8e-06,0.035,0.035,0.01,0.0012,6.6e-05,0.0013,0.00077,0.0013,0.0013,1,1 +11890000,0.78,-0.021,0.0038,-0.63,-0.12,-0.019,-0.01,-0.019,-0.0015,-3.7e+02,-0.0008,-0.0061,-9.9e-05,0.00091,-0.036,-0.11,-0.11,-0.023,0.5,-0.0015,-0.089,-0.069,0,0,0.00089,0.00092,0.047,0.09,0.09,0.037,0.067,0.067,0.063,9.5e-06,1e-05,4.8e-06,0.035,0.035,0.01,0.0012,6.6e-05,0.0013,0.00077,0.0013,0.0013,1,1 +11990000,0.78,-0.02,0.003,-0.63,-0.095,-0.012,-0.015,-0.011,0.0012,-3.7e+02,-0.00095,-0.0061,-9.3e-05,0.0013,-0.04,-0.11,-0.11,-0.023,0.5,-0.0014,-0.089,-0.07,0,0,0.00079,0.00082,0.047,0.074,0.074,0.033,0.055,0.055,0.061,9e-06,9.7e-06,4.8e-06,0.034,0.034,0.01,0.0012,6.6e-05,0.0013,0.00076,0.0013,0.0013,1,1 +12090000,0.78,-0.02,0.0029,-0.63,-0.11,-0.016,-0.021,-0.021,-0.00045,-3.7e+02,-0.00099,-0.006,-9e-05,0.0018,-0.04,-0.11,-0.11,-0.023,0.5,-0.0013,-0.089,-0.07,0,0,0.00079,0.00082,0.047,0.087,0.086,0.031,0.063,0.063,0.061,8.6e-06,9.2e-06,4.8e-06,0.034,0.034,0.01,0.0012,6.6e-05,0.0013,0.00076,0.0013,0.0013,1,1 +12190000,0.78,-0.019,0.0023,-0.63,-0.084,-0.015,-0.016,-0.01,0.00028,-3.7e+02,-0.00098,-0.006,-8.9e-05,0.0016,-0.046,-0.11,-0.11,-0.023,0.5,-0.0013,-0.09,-0.07,0,0,0.00071,0.00073,0.046,0.071,0.071,0.028,0.052,0.052,0.059,8.1e-06,8.7e-06,4.8e-06,0.034,0.034,0.01,0.0012,6.5e-05,0.0012,0.00074,0.0013,0.0012,1,1 +12290000,0.78,-0.019,0.0024,-0.63,-0.092,-0.017,-0.015,-0.019,-0.0014,-3.7e+02,-0.00094,-0.006,-8.9e-05,0.0017,-0.046,-0.11,-0.11,-0.023,0.5,-0.0013,-0.09,-0.07,0,0,0.00071,0.00073,0.046,0.082,0.082,0.028,0.06,0.06,0.059,7.8e-06,8.4e-06,4.8e-06,0.034,0.034,0.01,0.0012,6.5e-05,0.0012,0.00074,0.0013,0.0012,1,1 +12390000,0.78,-0.019,0.0019,-0.63,-0.073,-0.014,-0.013,-0.0094,-0.00012,-3.7e+02,-0.00099,-0.006,-8.8e-05,0.0011,-0.05,-0.11,-0.11,-0.024,0.5,-0.0014,-0.09,-0.07,0,0,0.00064,0.00066,0.046,0.067,0.067,0.026,0.05,0.05,0.057,7.4e-06,8e-06,4.8e-06,0.033,0.033,0.01,0.0012,6.5e-05,0.0012,0.00073,0.0013,0.0012,1,1 +12490000,0.78,-0.019,0.0021,-0.63,-0.08,-0.016,-0.016,-0.017,-0.0014,-3.7e+02,-0.00096,-0.0061,-9.1e-05,0.00059,-0.05,-0.11,-0.11,-0.024,0.5,-0.0015,-0.09,-0.07,0,0,0.00064,0.00066,0.046,0.077,0.076,0.026,0.058,0.058,0.057,7.1e-06,7.6e-06,4.8e-06,0.033,0.033,0.01,0.0012,6.5e-05,0.0012,0.00073,0.0013,0.0012,1,1 +12590000,0.78,-0.018,0.0018,-0.63,-0.073,-0.014,-0.022,-0.014,-0.00033,-3.7e+02,-0.001,-0.0061,-8.8e-05,0.00072,-0.052,-0.11,-0.11,-0.024,0.5,-0.0015,-0.09,-0.07,0,0,0.00059,0.0006,0.046,0.063,0.063,0.025,0.049,0.049,0.055,6.8e-06,7.3e-06,4.8e-06,0.033,0.033,0.0099,0.0012,6.5e-05,0.0012,0.00072,0.0013,0.0012,1,1 +12690000,0.78,-0.018,0.0018,-0.63,-0.079,-0.015,-0.025,-0.021,-0.0015,-3.7e+02,-0.0011,-0.0061,-8.8e-05,0.00038,-0.051,-0.11,-0.11,-0.024,0.5,-0.0015,-0.09,-0.07,0,0,0.00059,0.0006,0.046,0.071,0.071,0.025,0.057,0.057,0.055,6.5e-06,7e-06,4.8e-06,0.033,0.033,0.0099,0.0012,6.4e-05,0.0012,0.00072,0.0013,0.0012,1,1 +12790000,0.78,-0.018,0.0016,-0.63,-0.072,-0.012,-0.028,-0.018,-0.0006,-3.7e+02,-0.0011,-0.0061,-8.7e-05,0.00058,-0.053,-0.11,-0.11,-0.024,0.5,-0.0015,-0.09,-0.07,0,0,0.00054,0.00056,0.046,0.059,0.058,0.024,0.048,0.048,0.053,6.2e-06,6.7e-06,4.8e-06,0.032,0.032,0.0097,0.0012,6.4e-05,0.0012,0.00071,0.0013,0.0012,1,1 +12890000,0.78,-0.018,0.0017,-0.63,-0.079,-0.013,-0.027,-0.026,-0.0019,-3.7e+02,-0.001,-0.0061,-8.9e-05,0.00062,-0.054,-0.11,-0.11,-0.024,0.5,-0.0015,-0.09,-0.07,0,0,0.00055,0.00056,0.046,0.066,0.066,0.025,0.056,0.056,0.054,6e-06,6.5e-06,4.8e-06,0.032,0.032,0.0097,0.0012,6.4e-05,0.0012,0.00071,0.0013,0.0012,1,1 +12990000,0.78,-0.018,0.0013,-0.63,-0.064,-0.012,-0.028,-0.019,-0.0015,-3.7e+02,-0.0011,-0.006,-8.6e-05,0.0009,-0.056,-0.11,-0.11,-0.024,0.5,-0.0014,-0.09,-0.07,0,0,0.00052,0.00053,0.046,0.059,0.058,0.025,0.058,0.058,0.052,5.7e-06,6.2e-06,4.8e-06,0.032,0.032,0.0094,0.0012,6.4e-05,0.0012,0.00071,0.0013,0.0012,1,1 +13090000,0.78,-0.018,0.0014,-0.63,-0.069,-0.011,-0.028,-0.026,-0.0023,-3.7e+02,-0.001,-0.0061,-8.9e-05,0.00025,-0.057,-0.11,-0.11,-0.024,0.5,-0.0015,-0.09,-0.07,0,0,0.00052,0.00053,0.046,0.065,0.065,0.025,0.066,0.066,0.052,5.5e-06,6e-06,4.8e-06,0.032,0.032,0.0094,0.0012,6.4e-05,0.0012,0.0007,0.0013,0.0012,1,1 +13190000,0.78,-0.018,0.0011,-0.63,-0.055,-0.011,-0.025,-0.017,-0.0015,-3.7e+02,-0.0011,-0.0061,-8.7e-05,-1.3e-05,-0.058,-0.11,-0.11,-0.024,0.5,-0.0016,-0.091,-0.07,0,0,0.00049,0.00051,0.046,0.058,0.058,0.025,0.067,0.067,0.051,5.2e-06,5.7e-06,4.8e-06,0.032,0.032,0.0091,0.0012,6.4e-05,0.0012,0.0007,0.0013,0.0012,1,1 +13290000,0.78,-0.018,0.0011,-0.63,-0.06,-0.013,-0.021,-0.024,-0.003,-3.7e+02,-0.001,-0.006,-8.8e-05,0.00039,-0.059,-0.12,-0.11,-0.024,0.5,-0.0015,-0.091,-0.07,0,0,0.00049,0.00051,0.046,0.064,0.064,0.027,0.077,0.077,0.051,5.1e-06,5.5e-06,4.8e-06,0.032,0.032,0.0091,0.0012,6.3e-05,0.0012,0.0007,0.0013,0.0012,1,1 +13390000,0.78,-0.017,0.00099,-0.63,-0.049,-0.012,-0.017,-0.016,-0.002,-3.7e+02,-0.001,-0.006,-8.5e-05,0.00065,-0.06,-0.12,-0.11,-0.024,0.5,-0.0014,-0.091,-0.07,0,0,0.00047,0.00049,0.046,0.056,0.056,0.026,0.077,0.077,0.05,4.9e-06,5.3e-06,4.8e-06,0.032,0.032,0.0088,0.0012,6.3e-05,0.0012,0.00069,0.0013,0.0012,1,1 +13490000,0.78,-0.017,0.00096,-0.63,-0.053,-0.013,-0.016,-0.022,-0.0034,-3.7e+02,-0.001,-0.006,-8.5e-05,0.00089,-0.061,-0.12,-0.11,-0.024,0.5,-0.0014,-0.091,-0.07,0,0,0.00047,0.00049,0.046,0.062,0.062,0.028,0.088,0.088,0.05,4.7e-06,5.1e-06,4.8e-06,0.031,0.032,0.0087,0.0012,6.3e-05,0.0012,0.00069,0.0013,0.0012,1,1 +13590000,0.78,-0.017,0.00083,-0.63,-0.043,-0.012,-0.018,-0.014,-0.002,-3.7e+02,-0.001,-0.006,-8.5e-05,0.00051,-0.062,-0.12,-0.11,-0.024,0.5,-0.0015,-0.091,-0.07,0,0,0.00046,0.00047,0.046,0.054,0.054,0.028,0.087,0.087,0.05,4.5e-06,5e-06,4.8e-06,0.031,0.032,0.0084,0.0012,6.3e-05,0.0012,0.00069,0.0013,0.0012,1,1 +13690000,0.78,-0.017,0.00081,-0.63,-0.046,-0.015,-0.022,-0.019,-0.0035,-3.7e+02,-0.001,-0.006,-8.4e-05,0.00095,-0.062,-0.12,-0.11,-0.024,0.5,-0.0014,-0.091,-0.07,0,0,0.00046,0.00047,0.046,0.06,0.06,0.029,0.098,0.098,0.05,4.3e-06,4.8e-06,4.8e-06,0.031,0.032,0.0083,0.0012,6.3e-05,0.0012,0.00069,0.0013,0.0012,1,1 +13790000,0.78,-0.017,0.00063,-0.63,-0.033,-0.013,-0.024,-0.0063,-0.003,-3.7e+02,-0.0011,-0.006,-8.3e-05,0.00068,-0.062,-0.12,-0.11,-0.024,0.5,-0.0014,-0.091,-0.07,0,0,0.00044,0.00045,0.046,0.045,0.045,0.029,0.072,0.072,0.049,4.2e-06,4.6e-06,4.8e-06,0.031,0.031,0.0079,0.0012,6.3e-05,0.0012,0.00068,0.0013,0.0012,1,1 +13890000,0.78,-0.017,0.00069,-0.63,-0.037,-0.015,-0.028,-0.01,-0.0045,-3.7e+02,-0.001,-0.006,-8.4e-05,0.00091,-0.063,-0.12,-0.11,-0.024,0.5,-0.0014,-0.091,-0.07,0,0,0.00044,0.00045,0.046,0.049,0.049,0.03,0.081,0.081,0.05,4e-06,4.5e-06,4.8e-06,0.031,0.031,0.0078,0.0012,6.3e-05,0.0012,0.00068,0.0013,0.0012,1,1 +13990000,0.78,-0.017,0.00054,-0.63,-0.029,-0.014,-0.027,-0.0032,-0.004,-3.7e+02,-0.0011,-0.006,-8.4e-05,0.00078,-0.064,-0.12,-0.11,-0.024,0.5,-0.0014,-0.091,-0.07,0,0,0.00043,0.00044,0.046,0.04,0.04,0.03,0.063,0.063,0.05,3.9e-06,4.3e-06,4.8e-06,0.031,0.031,0.0074,0.0012,6.2e-05,0.0012,0.00068,0.0013,0.0012,1,1 +14090000,0.78,-0.017,0.00048,-0.63,-0.03,-0.015,-0.028,-0.006,-0.0056,-3.7e+02,-0.0011,-0.006,-8.1e-05,0.0012,-0.063,-0.12,-0.11,-0.024,0.5,-0.0014,-0.091,-0.07,0,0,0.00043,0.00044,0.046,0.044,0.044,0.031,0.07,0.07,0.05,3.8e-06,4.2e-06,4.8e-06,0.031,0.031,0.0073,0.0012,6.2e-05,0.0012,0.00068,0.0013,0.0012,1,1 +14190000,0.78,-0.017,0.00041,-0.63,-0.024,-0.013,-0.03,-8.4e-05,-0.0036,-3.7e+02,-0.0011,-0.0059,-8e-05,0.0015,-0.064,-0.12,-0.11,-0.024,0.5,-0.0013,-0.091,-0.069,0,0,0.00042,0.00043,0.046,0.036,0.036,0.03,0.057,0.057,0.05,3.6e-06,4.1e-06,4.8e-06,0.031,0.031,0.0069,0.0012,6.2e-05,0.0012,0.00068,0.0012,0.0012,1,1 +14290000,0.78,-0.017,0.00037,-0.63,-0.025,-0.015,-0.029,-0.0025,-0.0051,-3.7e+02,-0.0011,-0.0059,-8e-05,0.0016,-0.064,-0.12,-0.11,-0.024,0.5,-0.0013,-0.091,-0.069,0,0,0.00042,0.00043,0.046,0.04,0.04,0.032,0.064,0.064,0.051,3.5e-06,3.9e-06,4.8e-06,0.031,0.031,0.0067,0.0012,6.2e-05,0.0012,0.00068,0.0012,0.0012,1,1 +14390000,0.78,-0.017,0.00033,-0.63,-0.02,-0.015,-0.031,0.0017,-0.0037,-3.7e+02,-0.0011,-0.0059,-7.8e-05,0.0021,-0.065,-0.12,-0.11,-0.024,0.5,-0.0012,-0.091,-0.069,0,0,0.00041,0.00042,0.046,0.034,0.034,0.031,0.053,0.053,0.05,3.4e-06,3.8e-06,4.8e-06,0.031,0.031,0.0063,0.0012,6.2e-05,0.0012,0.00068,0.0012,0.0012,1,1 +14490000,0.78,-0.017,0.0004,-0.63,-0.022,-0.018,-0.034,-0.00071,-0.0054,-3.7e+02,-0.001,-0.0059,-7.9e-05,0.0022,-0.066,-0.12,-0.11,-0.024,0.5,-0.0012,-0.091,-0.069,0,0,0.00041,0.00042,0.046,0.037,0.037,0.032,0.06,0.06,0.051,3.3e-06,3.7e-06,4.8e-06,0.031,0.031,0.0062,0.0012,6.2e-05,0.0012,0.00068,0.0012,0.0012,1,1 +14590000,0.78,-0.017,0.00048,-0.63,-0.023,-0.018,-0.034,-0.0013,-0.0052,-3.7e+02,-0.001,-0.0059,-8e-05,0.0021,-0.066,-0.12,-0.11,-0.024,0.5,-0.0012,-0.091,-0.069,0,0,0.00041,0.00042,0.046,0.032,0.032,0.031,0.051,0.051,0.051,3.2e-06,3.6e-06,4.8e-06,0.031,0.031,0.0058,0.0012,6.2e-05,0.0012,0.00067,0.0012,0.0012,1,1 +14690000,0.78,-0.017,0.0005,-0.63,-0.026,-0.017,-0.031,-0.0038,-0.0071,-3.7e+02,-0.00099,-0.0059,-7.9e-05,0.0024,-0.066,-0.12,-0.11,-0.024,0.5,-0.0012,-0.091,-0.069,0,0,0.00041,0.00042,0.046,0.035,0.035,0.032,0.056,0.056,0.051,3.1e-06,3.5e-06,4.8e-06,0.031,0.031,0.0057,0.0012,6.2e-05,0.0012,0.00067,0.0012,0.0012,1,1 +14790000,0.78,-0.017,0.00051,-0.63,-0.025,-0.016,-0.027,-0.0038,-0.0066,-3.7e+02,-0.00099,-0.0059,-7.9e-05,0.0023,-0.066,-0.12,-0.11,-0.024,0.5,-0.0012,-0.091,-0.069,0,0,0.0004,0.00041,0.046,0.03,0.03,0.031,0.049,0.049,0.051,3e-06,3.3e-06,4.8e-06,0.031,0.031,0.0053,0.0012,6.2e-05,0.0012,0.00067,0.0012,0.0012,1,1 +14890000,0.78,-0.017,0.00052,-0.63,-0.028,-0.019,-0.03,-0.0065,-0.0085,-3.7e+02,-0.00097,-0.0059,-7.8e-05,0.0025,-0.066,-0.12,-0.11,-0.024,0.5,-0.0012,-0.091,-0.069,0,0,0.0004,0.00041,0.046,0.033,0.033,0.031,0.054,0.054,0.052,2.9e-06,3.3e-06,4.8e-06,0.031,0.031,0.0051,0.0012,6.2e-05,0.0012,0.00067,0.0012,0.0012,1,1 +14990000,0.78,-0.017,0.00057,-0.63,-0.026,-0.016,-0.026,-0.0049,-0.0065,-3.7e+02,-0.00097,-0.0059,-7.8e-05,0.0024,-0.067,-0.12,-0.11,-0.024,0.5,-0.0012,-0.091,-0.069,0,0,0.0004,0.00041,0.046,0.029,0.029,0.03,0.047,0.047,0.051,2.8e-06,3.2e-06,4.8e-06,0.031,0.031,0.0048,0.0012,6.1e-05,0.0012,0.00067,0.0012,0.0012,1,1 +15090000,0.78,-0.017,0.00066,-0.63,-0.028,-0.017,-0.029,-0.0076,-0.0081,-3.7e+02,-0.00097,-0.0059,-7.9e-05,0.0022,-0.067,-0.12,-0.11,-0.024,0.5,-0.0012,-0.091,-0.069,0,0,0.0004,0.00041,0.046,0.031,0.031,0.031,0.052,0.052,0.052,2.7e-06,3.1e-06,4.8e-06,0.031,0.031,0.0046,0.0012,6.1e-05,0.0012,0.00067,0.0012,0.0012,1,1 +15190000,0.78,-0.017,0.00069,-0.63,-0.026,-0.016,-0.026,-0.006,-0.0064,-3.7e+02,-0.00096,-0.0059,-7.9e-05,0.0022,-0.067,-0.12,-0.11,-0.024,0.5,-0.0011,-0.091,-0.069,0,0,0.00039,0.00041,0.046,0.027,0.028,0.03,0.046,0.046,0.052,2.6e-06,3e-06,4.8e-06,0.031,0.031,0.0043,0.0012,6.1e-05,0.0012,0.00067,0.0012,0.0012,1,1 +15290000,0.78,-0.017,0.00069,-0.63,-0.028,-0.018,-0.024,-0.0086,-0.0082,-3.7e+02,-0.00097,-0.0059,-7.8e-05,0.0023,-0.067,-0.12,-0.11,-0.024,0.5,-0.0011,-0.09,-0.069,0,0,0.00039,0.00041,0.046,0.03,0.03,0.03,0.051,0.051,0.052,2.5e-06,2.9e-06,4.8e-06,0.031,0.031,0.0042,0.0012,6.1e-05,0.0012,0.00067,0.0012,0.0012,1,1 +15390000,0.78,-0.017,0.00064,-0.63,-0.027,-0.018,-0.022,-0.0082,-0.0083,-3.7e+02,-0.00099,-0.0059,-7.4e-05,0.0028,-0.067,-0.13,-0.11,-0.024,0.5,-0.0011,-0.09,-0.069,0,0,0.00039,0.00041,0.046,0.029,0.029,0.029,0.054,0.054,0.051,2.4e-06,2.8e-06,4.8e-06,0.031,0.031,0.0039,0.0012,6.1e-05,0.0012,0.00067,0.0012,0.0012,1,1 +15490000,0.78,-0.017,0.00066,-0.63,-0.03,-0.018,-0.022,-0.011,-0.0098,-3.7e+02,-0.00099,-0.0059,-7.6e-05,0.0024,-0.067,-0.12,-0.11,-0.024,0.5,-0.0011,-0.09,-0.069,0,0,0.00039,0.00041,0.046,0.031,0.031,0.029,0.06,0.06,0.053,2.4e-06,2.7e-06,4.8e-06,0.031,0.031,0.0037,0.0012,6.1e-05,0.0012,0.00067,0.0012,0.0012,1,1 +15590000,0.78,-0.017,0.0007,-0.63,-0.028,-0.017,-0.021,-0.01,-0.0091,-3.7e+02,-0.001,-0.006,-7.6e-05,0.0022,-0.067,-0.13,-0.11,-0.024,0.5,-0.0011,-0.091,-0.069,0,0,0.00039,0.0004,0.046,0.029,0.03,0.028,0.062,0.062,0.052,2.3e-06,2.7e-06,4.8e-06,0.031,0.031,0.0035,0.0012,6.1e-05,0.0012,0.00067,0.0012,0.0012,1,1 +15690000,0.78,-0.017,0.00067,-0.63,-0.029,-0.017,-0.021,-0.013,-0.011,-3.7e+02,-0.001,-0.006,-7.6e-05,0.0021,-0.067,-0.13,-0.11,-0.024,0.5,-0.0011,-0.091,-0.069,0,0,0.00039,0.0004,0.046,0.032,0.032,0.028,0.069,0.069,0.052,2.2e-06,2.6e-06,4.8e-06,0.03,0.031,0.0033,0.0012,6.1e-05,0.0012,0.00067,0.0012,0.0012,1,1 +15790000,0.78,-0.017,0.00067,-0.63,-0.026,-0.016,-0.024,-0.009,-0.009,-3.7e+02,-0.001,-0.006,-7.6e-05,0.002,-0.067,-0.12,-0.11,-0.024,0.5,-0.0011,-0.091,-0.069,0,0,0.00039,0.0004,0.046,0.026,0.027,0.027,0.056,0.057,0.051,2.2e-06,2.5e-06,4.8e-06,0.03,0.031,0.0031,0.0012,6.1e-05,0.0012,0.00066,0.0012,0.0012,1,1 +15890000,0.78,-0.017,0.00068,-0.63,-0.028,-0.017,-0.022,-0.012,-0.011,-3.7e+02,-0.001,-0.006,-7.6e-05,0.002,-0.067,-0.12,-0.11,-0.024,0.5,-0.0011,-0.091,-0.069,0,0,0.00039,0.0004,0.046,0.028,0.029,0.027,0.063,0.063,0.052,2.1e-06,2.5e-06,4.8e-06,0.03,0.031,0.003,0.0012,6.1e-05,0.0012,0.00066,0.0012,0.0012,1,1 +15990000,0.78,-0.017,0.00066,-0.63,-0.025,-0.017,-0.017,-0.0084,-0.0095,-3.7e+02,-0.001,-0.0059,-7.3e-05,0.0023,-0.067,-0.13,-0.11,-0.024,0.5,-0.0011,-0.091,-0.069,0,0,0.00038,0.0004,0.046,0.024,0.025,0.026,0.053,0.053,0.051,2e-06,2.4e-06,4.8e-06,0.03,0.031,0.0028,0.0012,6.1e-05,0.0012,0.00066,0.0012,0.0012,1,1 +16090000,0.78,-0.017,0.00058,-0.63,-0.027,-0.019,-0.014,-0.011,-0.012,-3.7e+02,-0.0011,-0.0059,-7.1e-05,0.0027,-0.067,-0.13,-0.11,-0.024,0.5,-0.0011,-0.091,-0.069,0,0,0.00038,0.0004,0.046,0.026,0.026,0.025,0.058,0.058,0.052,2e-06,2.3e-06,4.8e-06,0.03,0.031,0.0027,0.0012,6.1e-05,0.0012,0.00066,0.0012,0.0012,1,1 +16190000,0.78,-0.017,0.00056,-0.63,-0.025,-0.017,-0.013,-0.008,-0.009,-3.7e+02,-0.0011,-0.0059,-6.9e-05,0.0027,-0.067,-0.13,-0.11,-0.024,0.5,-0.001,-0.091,-0.069,0,0,0.00038,0.0004,0.046,0.023,0.023,0.025,0.05,0.05,0.051,1.9e-06,2.3e-06,4.8e-06,0.03,0.031,0.0025,0.0012,6.1e-05,0.0012,0.00066,0.0012,0.0012,1,1 +16290000,0.78,-0.017,0.00049,-0.63,-0.028,-0.019,-0.014,-0.011,-0.011,-3.7e+02,-0.0011,-0.0059,-6.7e-05,0.0031,-0.067,-0.13,-0.11,-0.024,0.5,-0.001,-0.091,-0.069,0,0,0.00038,0.0004,0.046,0.024,0.025,0.024,0.055,0.055,0.052,1.9e-06,2.2e-06,4.8e-06,0.03,0.031,0.0024,0.0012,6e-05,0.0012,0.00066,0.0012,0.0012,1,1 +16390000,0.78,-0.017,0.00051,-0.63,-0.024,-0.015,-0.013,-0.0081,-0.0086,-3.7e+02,-0.0011,-0.0059,-6.5e-05,0.003,-0.066,-0.13,-0.11,-0.024,0.5,-0.001,-0.091,-0.069,0,0,0.00038,0.0004,0.046,0.022,0.022,0.023,0.047,0.047,0.051,1.8e-06,2.1e-06,4.8e-06,0.03,0.031,0.0022,0.0012,6e-05,0.0012,0.00066,0.0012,0.0012,1,1 +16490000,0.78,-0.016,0.00047,-0.63,-0.024,-0.017,-0.016,-0.01,-0.01,-3.7e+02,-0.0011,-0.0059,-6.5e-05,0.003,-0.066,-0.13,-0.11,-0.024,0.5,-0.001,-0.091,-0.069,0,0,0.00038,0.0004,0.046,0.023,0.024,0.023,0.052,0.052,0.052,1.8e-06,2.1e-06,4.8e-06,0.03,0.031,0.0022,0.0012,6e-05,0.0012,0.00066,0.0012,0.0012,1,1 +16590000,0.78,-0.016,0.00043,-0.63,-0.024,-0.013,-0.017,-0.01,-0.0062,-3.7e+02,-0.0011,-0.0059,-5.9e-05,0.0031,-0.066,-0.13,-0.11,-0.024,0.5,-0.00095,-0.091,-0.069,0,0,0.00038,0.00039,0.046,0.021,0.021,0.022,0.046,0.046,0.051,1.7e-06,2e-06,4.8e-06,0.03,0.031,0.002,0.0012,6e-05,0.0012,0.00066,0.0012,0.0012,1,1 +16690000,0.78,-0.016,0.00048,-0.63,-0.025,-0.014,-0.013,-0.013,-0.0073,-3.7e+02,-0.0011,-0.0059,-6.1e-05,0.0028,-0.066,-0.13,-0.11,-0.024,0.5,-0.00096,-0.091,-0.069,0,0,0.00038,0.00039,0.046,0.022,0.023,0.022,0.05,0.051,0.051,1.7e-06,2e-06,4.8e-06,0.03,0.031,0.0019,0.0012,6e-05,0.0012,0.00066,0.0012,0.0012,1,1 +16790000,0.78,-0.016,0.00052,-0.63,-0.024,-0.01,-0.012,-0.013,-0.004,-3.7e+02,-0.0011,-0.006,-5.6e-05,0.0028,-0.066,-0.13,-0.11,-0.024,0.5,-0.0009,-0.091,-0.069,0,0,0.00038,0.00039,0.046,0.02,0.02,0.021,0.044,0.044,0.05,1.6e-06,2e-06,4.8e-06,0.03,0.031,0.0018,0.0012,6e-05,0.0012,0.00066,0.0012,0.0012,1,1 +16890000,0.78,-0.016,0.00051,-0.63,-0.025,-0.011,-0.0096,-0.015,-0.0049,-3.7e+02,-0.0011,-0.006,-5.7e-05,0.0027,-0.066,-0.13,-0.11,-0.024,0.5,-0.00091,-0.091,-0.069,0,0,0.00038,0.00039,0.046,0.021,0.022,0.021,0.049,0.049,0.051,1.6e-06,1.9e-06,4.8e-06,0.03,0.031,0.0017,0.0012,6e-05,0.0012,0.00066,0.0012,0.0012,1,1 +16990000,0.78,-0.016,0.00055,-0.63,-0.024,-0.011,-0.0091,-0.014,-0.0048,-3.7e+02,-0.0012,-0.006,-5.9e-05,0.0025,-0.066,-0.13,-0.11,-0.024,0.5,-0.00092,-0.091,-0.069,0,0,0.00038,0.00039,0.046,0.019,0.02,0.02,0.043,0.043,0.05,1.5e-06,1.9e-06,4.8e-06,0.03,0.031,0.0016,0.0012,6e-05,0.0012,0.00066,0.0012,0.0012,1,1 +17090000,0.78,-0.016,0.00054,-0.63,-0.025,-0.013,-0.009,-0.016,-0.0059,-3.7e+02,-0.0012,-0.006,-5.8e-05,0.0025,-0.066,-0.13,-0.11,-0.024,0.5,-0.00093,-0.091,-0.069,0,0,0.00038,0.00039,0.046,0.02,0.021,0.02,0.048,0.048,0.05,1.5e-06,1.8e-06,4.8e-06,0.03,0.031,0.0016,0.0012,6e-05,0.0012,0.00066,0.0012,0.0012,1,1 +17190000,0.78,-0.016,0.00048,-0.63,-0.024,-0.015,-0.0099,-0.014,-0.0062,-3.7e+02,-0.0012,-0.006,-5.7e-05,0.0026,-0.066,-0.13,-0.11,-0.024,0.5,-0.00092,-0.091,-0.069,0,0,0.00037,0.00039,0.046,0.018,0.019,0.019,0.042,0.043,0.049,1.5e-06,1.8e-06,4.8e-06,0.03,0.031,0.0015,0.0012,6e-05,0.0012,0.00066,0.0012,0.0012,1,1 +17290000,0.78,-0.016,0.00051,-0.63,-0.027,-0.017,-0.0054,-0.017,-0.0074,-3.7e+02,-0.0012,-0.006,-5.8e-05,0.0024,-0.065,-0.13,-0.11,-0.024,0.5,-0.00093,-0.091,-0.069,0,0,0.00037,0.00039,0.046,0.02,0.02,0.019,0.047,0.047,0.049,1.4e-06,1.7e-06,4.8e-06,0.03,0.031,0.0014,0.0012,6e-05,0.0012,0.00066,0.0012,0.0012,1,1 +17390000,0.78,-0.016,0.00044,-0.63,-0.024,-0.018,-0.0035,-0.014,-0.0076,-3.7e+02,-0.0012,-0.006,-5.6e-05,0.0026,-0.065,-0.13,-0.11,-0.024,0.5,-0.00093,-0.091,-0.069,0,0,0.00037,0.00039,0.046,0.018,0.018,0.018,0.042,0.042,0.048,1.4e-06,1.7e-06,4.8e-06,0.03,0.031,0.0013,0.0012,6e-05,0.0012,0.00066,0.0012,0.0012,1,1 +17490000,0.78,-0.016,0.00046,-0.63,-0.026,-0.019,-0.0018,-0.017,-0.0095,-3.7e+02,-0.0012,-0.006,-5.6e-05,0.0026,-0.066,-0.13,-0.11,-0.024,0.5,-0.00092,-0.091,-0.069,0,0,0.00037,0.00039,0.046,0.019,0.02,0.018,0.046,0.046,0.049,1.4e-06,1.7e-06,4.8e-06,0.03,0.031,0.0013,0.0012,6e-05,0.0012,0.00066,0.0012,0.0012,1,1 +17590000,0.78,-0.016,0.00047,-0.63,-0.024,-0.019,0.0036,-0.014,-0.0091,-3.7e+02,-0.0012,-0.006,-5.5e-05,0.0026,-0.065,-0.13,-0.11,-0.024,0.5,-0.00091,-0.091,-0.069,0,0,0.00037,0.00039,0.046,0.017,0.018,0.017,0.041,0.041,0.048,1.3e-06,1.6e-06,4.8e-06,0.03,0.031,0.0012,0.0012,6e-05,0.0012,0.00066,0.0012,0.0012,1,1 +17690000,0.78,-0.016,0.00049,-0.63,-0.026,-0.021,0.003,-0.017,-0.011,-3.7e+02,-0.0012,-0.006,-5.4e-05,0.0027,-0.065,-0.13,-0.11,-0.024,0.5,-0.00091,-0.091,-0.069,0,0,0.00037,0.00039,0.046,0.019,0.019,0.017,0.045,0.045,0.048,1.3e-06,1.6e-06,4.8e-06,0.03,0.031,0.0012,0.0012,6e-05,0.0012,0.00066,0.0012,0.0012,1,1 +17790000,0.78,-0.016,0.00043,-0.63,-0.024,-0.022,0.0016,-0.015,-0.012,-3.7e+02,-0.0012,-0.0059,-4.8e-05,0.0031,-0.065,-0.13,-0.11,-0.024,0.5,-0.00088,-0.091,-0.069,0,0,0.00037,0.00039,0.046,0.018,0.019,0.016,0.048,0.048,0.048,1.3e-06,1.6e-06,4.8e-06,0.03,0.031,0.0011,0.0012,6e-05,0.0012,0.00066,0.0012,0.0012,1,1 +17890000,0.78,-0.016,0.00042,-0.63,-0.027,-0.023,0.0017,-0.018,-0.015,-3.7e+02,-0.0012,-0.0059,-4.6e-05,0.0033,-0.065,-0.13,-0.11,-0.024,0.5,-0.00087,-0.091,-0.069,0,0,0.00037,0.00039,0.046,0.02,0.02,0.016,0.053,0.053,0.048,1.2e-06,1.5e-06,4.8e-06,0.03,0.031,0.0011,0.0012,6e-05,0.0012,0.00066,0.0012,0.0012,1,1 +17990000,0.78,-0.016,0.00045,-0.63,-0.026,-0.021,0.0029,-0.016,-0.015,-3.7e+02,-0.0012,-0.0059,-4.5e-05,0.0032,-0.066,-0.13,-0.11,-0.024,0.5,-0.00086,-0.091,-0.069,0,0,0.00037,0.00039,0.046,0.019,0.02,0.016,0.055,0.055,0.047,1.2e-06,1.5e-06,4.8e-06,0.03,0.031,0.001,0.0012,5.9e-05,0.0012,0.00066,0.0012,0.0012,1,1 +18090000,0.78,-0.016,0.00049,-0.63,-0.027,-0.021,0.0053,-0.019,-0.016,-3.7e+02,-0.0012,-0.006,-4.8e-05,0.0029,-0.066,-0.13,-0.11,-0.024,0.5,-0.00087,-0.091,-0.069,0,0,0.00037,0.00039,0.046,0.021,0.021,0.016,0.06,0.061,0.047,1.2e-06,1.5e-06,4.8e-06,0.03,0.031,0.00098,0.0012,5.9e-05,0.0012,0.00066,0.0012,0.0012,1,1 +18190000,0.78,-0.016,0.00049,-0.63,-0.024,-0.02,0.0066,-0.014,-0.013,-3.7e+02,-0.0012,-0.006,-4.2e-05,0.003,-0.066,-0.13,-0.11,-0.024,0.5,-0.00084,-0.091,-0.069,0,0,0.00037,0.00039,0.046,0.018,0.019,0.015,0.051,0.051,0.047,1.1e-06,1.4e-06,4.8e-06,0.03,0.03,0.00093,0.0012,5.9e-05,0.0012,0.00065,0.0012,0.0012,1,1 +18290000,0.78,-0.016,0.00059,-0.63,-0.025,-0.02,0.0078,-0.016,-0.015,-3.7e+02,-0.0012,-0.006,-4.4e-05,0.0029,-0.066,-0.13,-0.11,-0.024,0.5,-0.00084,-0.091,-0.069,0,0,0.00037,0.00039,0.046,0.019,0.02,0.015,0.056,0.056,0.046,1.1e-06,1.4e-06,4.8e-06,0.03,0.03,0.00089,0.0012,5.9e-05,0.0012,0.00065,0.0012,0.0012,1,1 +18390000,0.78,-0.016,0.00055,-0.63,-0.024,-0.021,0.009,-0.013,-0.012,-3.7e+02,-0.0012,-0.006,-3.8e-05,0.0031,-0.066,-0.13,-0.11,-0.024,0.5,-0.0008,-0.091,-0.069,0,0,0.00037,0.00039,0.046,0.017,0.018,0.014,0.048,0.048,0.046,1.1e-06,1.4e-06,4.7e-06,0.03,0.03,0.00085,0.0012,5.9e-05,0.0012,0.00065,0.0012,0.0012,1,1 +18490000,0.78,-0.016,0.00051,-0.63,-0.024,-0.023,0.0086,-0.015,-0.015,-3.7e+02,-0.0012,-0.006,-3.7e-05,0.0031,-0.066,-0.13,-0.11,-0.024,0.5,-0.0008,-0.091,-0.069,0,0,0.00037,0.00039,0.046,0.018,0.019,0.014,0.053,0.053,0.046,1.1e-06,1.3e-06,4.7e-06,0.03,0.03,0.00082,0.0012,5.9e-05,0.0012,0.00065,0.0012,0.0012,1,1 +18590000,0.78,-0.016,0.00049,-0.63,-0.022,-0.022,0.0067,-0.012,-0.013,-3.7e+02,-0.0013,-0.0059,-2.8e-05,0.0034,-0.066,-0.13,-0.11,-0.024,0.5,-0.00076,-0.091,-0.069,0,0,0.00037,0.00039,0.046,0.016,0.017,0.014,0.046,0.046,0.045,1e-06,1.3e-06,4.7e-06,0.03,0.03,0.00078,0.0012,5.9e-05,0.0012,0.00065,0.0012,0.0012,1,1 +18690000,0.78,-0.016,0.00055,-0.63,-0.024,-0.023,0.0048,-0.015,-0.015,-3.7e+02,-0.0012,-0.006,-3e-05,0.0033,-0.066,-0.13,-0.11,-0.024,0.5,-0.00076,-0.091,-0.069,0,0,0.00037,0.00039,0.046,0.017,0.018,0.013,0.05,0.05,0.045,1e-06,1.3e-06,4.7e-06,0.03,0.03,0.00076,0.0012,5.9e-05,0.0012,0.00065,0.0012,0.0012,1,1 +18790000,0.78,-0.016,0.00057,-0.63,-0.022,-0.021,0.0045,-0.012,-0.012,-3.7e+02,-0.0013,-0.006,-2.6e-05,0.0032,-0.066,-0.13,-0.11,-0.024,0.5,-0.00074,-0.091,-0.068,0,0,0.00037,0.00039,0.046,0.015,0.016,0.013,0.044,0.044,0.045,1e-06,1.2e-06,4.7e-06,0.03,0.03,0.00073,0.0012,5.9e-05,0.0012,0.00065,0.0012,0.0012,1,1 +18890000,0.78,-0.016,0.00048,-0.63,-0.022,-0.024,0.0051,-0.014,-0.015,-3.7e+02,-0.0013,-0.0059,-2.2e-05,0.0034,-0.066,-0.13,-0.11,-0.024,0.5,-0.00074,-0.091,-0.068,0,0,0.00037,0.00039,0.046,0.016,0.017,0.013,0.048,0.048,0.045,9.8e-07,1.2e-06,4.7e-06,0.03,0.03,0.0007,0.0012,5.9e-05,0.0012,0.00065,0.0012,0.0012,1,1 +18990000,0.78,-0.016,0.00042,-0.63,-0.019,-0.024,0.0037,-0.0098,-0.013,-3.7e+02,-0.0013,-0.0059,-1.5e-05,0.0035,-0.065,-0.13,-0.11,-0.024,0.5,-0.00071,-0.091,-0.068,0,0,0.00037,0.00039,0.046,0.015,0.016,0.012,0.043,0.043,0.044,9.5e-07,1.2e-06,4.7e-06,0.03,0.03,0.00067,0.0012,5.9e-05,0.0012,0.00065,0.0012,0.0012,1,1 +19090000,0.78,-0.016,0.00041,-0.63,-0.019,-0.025,0.0067,-0.011,-0.015,-3.7e+02,-0.0013,-0.0059,-1.5e-05,0.0035,-0.065,-0.13,-0.11,-0.024,0.5,-0.00071,-0.091,-0.068,0,0,0.00037,0.00039,0.046,0.016,0.017,0.012,0.046,0.047,0.044,9.4e-07,1.2e-06,4.7e-06,0.03,0.03,0.00065,0.0012,5.9e-05,0.0012,0.00065,0.0012,0.0012,1,1 +19190000,0.78,-0.016,0.00036,-0.63,-0.015,-0.024,0.0067,-0.0077,-0.013,-3.7e+02,-0.0013,-0.0059,-8.2e-06,0.0035,-0.065,-0.13,-0.11,-0.024,0.5,-0.00068,-0.091,-0.068,0,0,0.00036,0.00038,0.046,0.015,0.015,0.012,0.041,0.042,0.044,9.1e-07,1.2e-06,4.7e-06,0.03,0.03,0.00063,0.0012,5.9e-05,0.0012,0.00065,0.0012,0.0012,1,1 +19290000,0.78,-0.016,0.00037,-0.63,-0.016,-0.024,0.0094,-0.0095,-0.016,-3.7e+02,-0.0013,-0.006,-1.1e-05,0.0034,-0.065,-0.13,-0.11,-0.024,0.5,-0.00069,-0.091,-0.068,0,0,0.00036,0.00038,0.046,0.016,0.017,0.012,0.045,0.046,0.044,9e-07,1.1e-06,4.7e-06,0.03,0.03,0.00061,0.0012,5.9e-05,0.0012,0.00065,0.0012,0.0012,1,1 +19390000,0.78,-0.016,0.0004,-0.63,-0.015,-0.022,0.013,-0.0084,-0.014,-3.7e+02,-0.0013,-0.006,-2.4e-06,0.0035,-0.065,-0.13,-0.11,-0.024,0.5,-0.00065,-0.091,-0.068,0,0,0.00036,0.00038,0.046,0.014,0.015,0.012,0.04,0.041,0.043,8.8e-07,1.1e-06,4.6e-06,0.03,0.03,0.00058,0.0012,5.9e-05,0.0012,0.00065,0.0012,0.0012,1,1 +19490000,0.78,-0.016,0.00035,-0.63,-0.015,-0.024,0.0096,-0.01,-0.017,-3.7e+02,-0.0013,-0.0059,1.4e-06,0.0037,-0.065,-0.13,-0.11,-0.024,0.5,-0.00064,-0.091,-0.068,0,0,0.00036,0.00038,0.046,0.015,0.016,0.011,0.044,0.045,0.043,8.6e-07,1.1e-06,4.6e-06,0.03,0.03,0.00057,0.0012,5.9e-05,0.0012,0.00065,0.0012,0.0012,1,1 +19590000,0.78,-0.015,0.0003,-0.63,-0.013,-0.022,0.0088,-0.0085,-0.015,-3.7e+02,-0.0013,-0.0059,1.4e-05,0.0039,-0.065,-0.13,-0.11,-0.024,0.5,-0.0006,-0.091,-0.068,0,0,0.00036,0.00038,0.046,0.014,0.015,0.011,0.04,0.04,0.042,8.4e-07,1.1e-06,4.6e-06,0.03,0.03,0.00055,0.0012,5.9e-05,0.0012,0.00065,0.0012,0.0012,1,1 +19690000,0.78,-0.015,0.00028,-0.63,-0.014,-0.021,0.01,-0.0093,-0.017,-3.7e+02,-0.0013,-0.0059,1.1e-05,0.0037,-0.065,-0.13,-0.11,-0.024,0.5,-0.00061,-0.091,-0.068,0,0,0.00036,0.00038,0.046,0.015,0.016,0.011,0.043,0.044,0.042,8.3e-07,1.1e-06,4.6e-06,0.03,0.03,0.00053,0.0012,5.9e-05,0.0012,0.00065,0.0012,0.0012,1,1 +19790000,0.78,-0.015,0.00024,-0.63,-0.012,-0.018,0.011,-0.0079,-0.015,-3.7e+02,-0.0013,-0.006,1.8e-05,0.0037,-0.065,-0.13,-0.11,-0.024,0.5,-0.00058,-0.091,-0.068,0,0,0.00036,0.00038,0.046,0.014,0.015,0.011,0.039,0.039,0.042,8.1e-07,1e-06,4.6e-06,0.03,0.03,0.00051,0.0012,5.9e-05,0.0012,0.00065,0.0012,0.0012,1,1 +19890000,0.78,-0.015,0.00028,-0.63,-0.011,-0.02,0.012,-0.0096,-0.017,-3.7e+02,-0.0013,-0.0059,2.4e-05,0.004,-0.065,-0.13,-0.11,-0.024,0.5,-0.00056,-0.091,-0.068,0,0,0.00036,0.00038,0.046,0.015,0.016,0.011,0.043,0.043,0.042,8e-07,1e-06,4.6e-06,0.03,0.03,0.0005,0.0012,5.9e-05,0.0012,0.00065,0.0012,0.0012,1,1 +19990000,0.78,-0.016,0.00028,-0.63,-0.0096,-0.02,0.015,-0.0084,-0.016,-3.7e+02,-0.0013,-0.0059,3.9e-05,0.0043,-0.065,-0.13,-0.11,-0.024,0.5,-0.00051,-0.091,-0.068,0,0,0.00036,0.00038,0.046,0.014,0.015,0.01,0.039,0.039,0.041,7.7e-07,9.9e-07,4.6e-06,0.03,0.03,0.00048,0.0012,5.9e-05,0.0012,0.00065,0.0012,0.0012,1,1 +20090000,0.78,-0.016,0.00023,-0.63,-0.0096,-0.02,0.015,-0.0091,-0.019,-3.7e+02,-0.0013,-0.0059,4.7e-05,0.0046,-0.065,-0.13,-0.11,-0.024,0.5,-0.00051,-0.091,-0.068,0,0,0.00036,0.00038,0.046,0.015,0.016,0.01,0.042,0.043,0.042,7.7e-07,9.8e-07,4.6e-06,0.03,0.03,0.00047,0.0012,5.9e-05,0.0012,0.00065,0.0012,0.0012,1,1 +20190000,0.78,-0.016,0.00019,-0.63,-0.01,-0.019,0.017,-0.0092,-0.017,-3.7e+02,-0.0013,-0.0059,5.8e-05,0.0047,-0.065,-0.13,-0.11,-0.024,0.5,-0.00046,-0.091,-0.068,0,0,0.00036,0.00038,0.046,0.014,0.015,0.01,0.038,0.039,0.041,7.4e-07,9.5e-07,4.6e-06,0.03,0.03,0.00045,0.0012,5.8e-05,0.0012,0.00065,0.0012,0.0012,1,1 +20290000,0.78,-0.016,0.00019,-0.63,-0.0089,-0.019,0.015,-0.0096,-0.019,-3.7e+02,-0.0013,-0.0059,6.1e-05,0.0047,-0.065,-0.13,-0.11,-0.024,0.5,-0.00047,-0.091,-0.068,0,0,0.00036,0.00038,0.046,0.014,0.016,0.0099,0.042,0.042,0.041,7.4e-07,9.4e-07,4.6e-06,0.03,0.03,0.00044,0.0012,5.8e-05,0.0012,0.00065,0.0012,0.0012,1,1 +20390000,0.78,-0.016,0.00024,-0.63,-0.0085,-0.016,0.017,-0.0095,-0.017,-3.7e+02,-0.0013,-0.0059,6.8e-05,0.0047,-0.065,-0.13,-0.11,-0.024,0.5,-0.00043,-0.091,-0.068,0,0,0.00036,0.00038,0.046,0.013,0.014,0.0097,0.038,0.038,0.041,7.2e-07,9.2e-07,4.5e-06,0.03,0.03,0.00043,0.0012,5.8e-05,0.0012,0.00065,0.0012,0.0012,1,1 +20490000,0.78,-0.016,0.0002,-0.63,-0.0087,-0.016,0.017,-0.01,-0.018,-3.7e+02,-0.0013,-0.0059,6.5e-05,0.0046,-0.065,-0.13,-0.11,-0.024,0.5,-0.00043,-0.091,-0.068,0,0,0.00036,0.00038,0.046,0.014,0.015,0.0096,0.041,0.042,0.041,7.1e-07,9.1e-07,4.5e-06,0.03,0.03,0.00042,0.0012,5.8e-05,0.0012,0.00065,0.0012,0.0012,1,1 +20590000,0.78,-0.016,0.0002,-0.63,-0.0081,-0.014,0.014,-0.0089,-0.016,-3.7e+02,-0.0013,-0.0059,6.9e-05,0.0045,-0.065,-0.13,-0.11,-0.024,0.5,-0.00041,-0.091,-0.068,0,0,0.00036,0.00038,0.046,0.013,0.014,0.0093,0.038,0.038,0.04,6.9e-07,8.8e-07,4.5e-06,0.03,0.03,0.0004,0.0012,5.8e-05,0.0012,0.00065,0.0012,0.0012,1,1 +20690000,0.78,-0.016,0.00015,-0.63,-0.0089,-0.014,0.015,-0.0098,-0.017,-3.7e+02,-0.0013,-0.0059,7.2e-05,0.0045,-0.065,-0.13,-0.11,-0.024,0.5,-0.00041,-0.091,-0.068,0,0,0.00036,0.00038,0.046,0.014,0.015,0.0093,0.041,0.042,0.04,6.8e-07,8.8e-07,4.5e-06,0.03,0.03,0.0004,0.0012,5.8e-05,0.0012,0.00065,0.0012,0.0012,1,1 +20790000,0.78,-0.016,0.00013,-0.63,-0.0065,-0.013,0.016,-0.0082,-0.015,-3.7e+02,-0.0013,-0.0059,7.9e-05,0.0045,-0.065,-0.13,-0.11,-0.024,0.5,-0.00039,-0.091,-0.068,0,0,0.00036,0.00038,0.046,0.013,0.014,0.0091,0.037,0.038,0.04,6.6e-07,8.5e-07,4.5e-06,0.03,0.03,0.00038,0.0012,5.8e-05,0.0012,0.00065,0.0012,0.0012,1,1 +20890000,0.78,-0.016,0.00011,-0.63,-0.0067,-0.013,0.015,-0.0088,-0.017,-3.7e+02,-0.0013,-0.0059,8.5e-05,0.0047,-0.065,-0.13,-0.11,-0.024,0.5,-0.00039,-0.091,-0.068,0,0,0.00036,0.00038,0.046,0.014,0.015,0.009,0.041,0.041,0.04,6.6e-07,8.4e-07,4.5e-06,0.03,0.03,0.00038,0.0012,5.8e-05,0.0012,0.00065,0.0012,0.0012,1,1 +20990000,0.78,-0.016,9.5e-05,-0.63,-0.0051,-0.011,0.015,-0.0084,-0.018,-3.7e+02,-0.0013,-0.0059,9e-05,0.0047,-0.065,-0.13,-0.11,-0.024,0.5,-0.00038,-0.091,-0.068,0,0,0.00036,0.00038,0.046,0.014,0.015,0.0088,0.043,0.044,0.039,6.4e-07,8.3e-07,4.4e-06,0.03,0.03,0.00037,0.0012,5.8e-05,0.0012,0.00065,0.0012,0.0012,1,1 +21090000,0.78,-0.016,8.8e-05,-0.63,-0.0062,-0.011,0.016,-0.0094,-0.019,-3.7e+02,-0.0013,-0.0059,9.3e-05,0.0048,-0.065,-0.13,-0.11,-0.024,0.5,-0.00037,-0.091,-0.068,0,0,0.00036,0.00038,0.046,0.015,0.016,0.0088,0.047,0.048,0.039,6.4e-07,8.2e-07,4.4e-06,0.03,0.03,0.00036,0.0012,5.8e-05,0.0012,0.00065,0.0012,0.0012,1,1 +21190000,0.78,-0.016,8.4e-05,-0.63,-0.0063,-0.011,0.015,-0.0099,-0.019,-3.7e+02,-0.0013,-0.0059,9.4e-05,0.0047,-0.065,-0.13,-0.11,-0.024,0.5,-0.00036,-0.091,-0.068,0,0,0.00036,0.00038,0.046,0.015,0.016,0.0086,0.049,0.05,0.039,6.2e-07,8e-07,4.4e-06,0.03,0.03,0.00035,0.0012,5.8e-05,0.0012,0.00065,0.0012,0.0012,1,1 +21290000,0.78,-0.016,-6.5e-06,-0.63,-0.0059,-0.011,0.017,-0.0099,-0.021,-3.7e+02,-0.0013,-0.0059,0.0001,0.0049,-0.065,-0.13,-0.11,-0.024,0.5,-0.00035,-0.091,-0.068,0,0,0.00036,0.00038,0.046,0.016,0.017,0.0085,0.054,0.055,0.039,6.2e-07,8e-07,4.4e-06,0.03,0.03,0.00034,0.0012,5.8e-05,0.0012,0.00065,0.0012,0.0012,1,1 +21390000,0.78,-0.016,4.3e-05,-0.63,-0.0052,-0.0064,0.016,-0.0087,-0.016,-3.7e+02,-0.0013,-0.0059,0.00011,0.0048,-0.065,-0.13,-0.11,-0.024,0.5,-0.00032,-0.091,-0.068,0,0,0.00036,0.00038,0.046,0.014,0.015,0.0084,0.046,0.047,0.039,6e-07,7.7e-07,4.4e-06,0.03,0.03,0.00033,0.0012,5.8e-05,0.0012,0.00065,0.0012,0.0012,1,1 +21490000,0.78,-0.016,3.3e-05,-0.63,-0.0059,-0.0073,0.016,-0.0098,-0.017,-3.7e+02,-0.0013,-0.0059,0.00011,0.0049,-0.065,-0.13,-0.11,-0.024,0.5,-0.00032,-0.091,-0.068,0,0,0.00036,0.00038,0.046,0.015,0.016,0.0083,0.05,0.052,0.038,5.9e-07,7.6e-07,4.4e-06,0.03,0.03,0.00033,0.0012,5.8e-05,0.0012,0.00065,0.0012,0.0012,1,1 +21590000,0.78,-0.016,7.2e-05,-0.63,-0.0045,-0.0056,0.016,-0.0082,-0.013,-3.7e+02,-0.0013,-0.0059,0.00012,0.0048,-0.065,-0.13,-0.11,-0.024,0.5,-0.00029,-0.091,-0.068,0,0,0.00036,0.00038,0.046,0.014,0.015,0.0081,0.044,0.045,0.038,5.8e-07,7.4e-07,4.3e-06,0.03,0.03,0.00032,0.0012,5.8e-05,0.0012,0.00065,0.0012,0.0012,1,1 +21690000,0.78,-0.016,6.5e-05,-0.63,-0.0061,-0.0065,0.017,-0.0095,-0.015,-3.7e+02,-0.0013,-0.0059,0.00012,0.0048,-0.066,-0.13,-0.11,-0.024,0.5,-0.00028,-0.091,-0.068,0,0,0.00036,0.00038,0.046,0.014,0.016,0.0081,0.048,0.049,0.038,5.7e-07,7.4e-07,4.3e-06,0.03,0.03,0.00031,0.0012,5.8e-05,0.0012,0.00065,0.0012,0.0012,1,1 +21790000,0.78,-0.016,0.00016,-0.63,-0.0051,-0.0043,0.016,-0.0083,-0.0092,-3.7e+02,-0.0013,-0.0059,0.00013,0.0047,-0.065,-0.13,-0.11,-0.024,0.5,-0.00024,-0.091,-0.068,0,0,0.00036,0.00038,0.046,0.013,0.014,0.008,0.042,0.043,0.038,5.6e-07,7.1e-07,4.3e-06,0.03,0.03,0.00031,0.0012,5.8e-05,0.0012,0.00065,0.0012,0.0012,1,1 +21890000,0.78,-0.015,0.00016,-0.63,-0.0058,-0.005,0.016,-0.009,-0.0097,-3.7e+02,-0.0013,-0.0059,0.00013,0.0047,-0.065,-0.13,-0.11,-0.024,0.5,-0.00024,-0.091,-0.068,0,0,0.00036,0.00038,0.046,0.014,0.015,0.0079,0.046,0.047,0.038,5.5e-07,7.1e-07,4.3e-06,0.03,0.03,0.0003,0.0012,5.8e-05,0.0012,0.00065,0.0012,0.0012,1,1 +21990000,0.78,-0.015,0.00019,-0.63,-0.0057,-0.0022,0.017,-0.0084,-0.0057,-3.7e+02,-0.0013,-0.0059,0.00014,0.0046,-0.065,-0.13,-0.11,-0.024,0.5,-0.00021,-0.091,-0.068,0,0,0.00036,0.00038,0.046,0.013,0.014,0.0078,0.041,0.042,0.038,5.4e-07,6.9e-07,4.3e-06,0.03,0.03,0.00029,0.0011,5.8e-05,0.0012,0.00065,0.0012,0.0012,1,1 +22090000,0.78,-0.015,0.00018,-0.63,-0.0054,-0.0037,0.015,-0.0088,-0.006,-3.7e+02,-0.0013,-0.0059,0.00014,0.0046,-0.065,-0.13,-0.11,-0.024,0.5,-0.00021,-0.091,-0.068,0,0,0.00036,0.00038,0.046,0.014,0.015,0.0078,0.045,0.046,0.037,5.4e-07,6.9e-07,4.2e-06,0.03,0.03,0.00029,0.0011,5.8e-05,0.0012,0.00065,0.0012,0.0012,1,1 +22190000,0.78,-0.015,0.00019,-0.63,-0.004,-0.0043,0.016,-0.0073,-0.0054,-3.7e+02,-0.0013,-0.0059,0.00015,0.0046,-0.065,-0.13,-0.11,-0.024,0.5,-0.0002,-0.091,-0.068,0,0,0.00036,0.00038,0.046,0.013,0.014,0.0076,0.04,0.041,0.037,5.2e-07,6.7e-07,4.2e-06,0.03,0.03,0.00028,0.0011,5.8e-05,0.0012,0.00065,0.0012,0.0012,1,1 +22290000,0.78,-0.015,0.00016,-0.63,-0.0036,-0.0039,0.016,-0.008,-0.0057,-3.7e+02,-0.0013,-0.0059,0.00015,0.0046,-0.065,-0.13,-0.11,-0.024,0.5,-0.00019,-0.091,-0.068,0,0,0.00036,0.00038,0.046,0.013,0.015,0.0076,0.043,0.045,0.037,5.2e-07,6.6e-07,4.2e-06,0.03,0.03,0.00028,0.0011,5.8e-05,0.0012,0.00065,0.0012,0.0012,1,1 +22390000,0.78,-0.015,0.00015,-0.63,-0.0011,-0.0039,0.017,-0.0062,-0.0051,-3.7e+02,-0.0014,-0.0059,0.00015,0.0046,-0.065,-0.13,-0.11,-0.024,0.5,-0.00019,-0.091,-0.068,0,0,0.00036,0.00038,0.046,0.012,0.014,0.0075,0.039,0.04,0.037,5.1e-07,6.5e-07,4.2e-06,0.03,0.03,0.00027,0.0011,5.8e-05,0.0012,0.00064,0.0012,0.0012,1,1 +22490000,0.78,-0.015,0.00013,-0.63,7e-05,-0.0045,0.018,-0.0056,-0.0054,-3.7e+02,-0.0014,-0.0059,0.00015,0.0046,-0.065,-0.13,-0.11,-0.024,0.5,-0.0002,-0.091,-0.068,0,0,0.00036,0.00038,0.046,0.013,0.015,0.0074,0.042,0.044,0.037,5e-07,6.4e-07,4.2e-06,0.03,0.03,0.00027,0.0011,5.8e-05,0.0012,0.00064,0.0012,0.0012,1,1 +22590000,0.78,-0.015,0.00013,-0.63,0.0019,-0.0034,0.018,-0.0039,-0.0047,-3.7e+02,-0.0014,-0.0059,0.00016,0.0046,-0.065,-0.13,-0.11,-0.024,0.5,-0.00019,-0.091,-0.068,0,0,0.00036,0.00038,0.046,0.013,0.015,0.0073,0.045,0.046,0.036,4.9e-07,6.3e-07,4.1e-06,0.03,0.03,0.00026,0.0011,5.8e-05,0.0012,0.00064,0.0012,0.0012,1,1 +22690000,0.78,-0.015,6.1e-05,-0.63,0.0035,-0.0046,0.019,-0.0032,-0.0056,-3.7e+02,-0.0014,-0.0059,0.00016,0.0047,-0.065,-0.13,-0.11,-0.024,0.5,-0.00019,-0.091,-0.068,0,0,0.00036,0.00038,0.046,0.014,0.016,0.0073,0.048,0.05,0.036,4.9e-07,6.3e-07,4.1e-06,0.03,0.03,0.00026,0.0011,5.8e-05,0.0012,0.00064,0.0012,0.0012,1,1 +22790000,0.78,-0.015,0.0001,-0.63,0.0045,-0.004,0.02,-0.0026,-0.0042,-3.7e+02,-0.0014,-0.0059,0.00015,0.0045,-0.065,-0.13,-0.11,-0.024,0.5,-0.0002,-0.091,-0.068,0,0,0.00036,0.00038,0.046,0.014,0.016,0.0072,0.051,0.052,0.036,4.8e-07,6.2e-07,4.1e-06,0.03,0.03,0.00025,0.0011,5.8e-05,0.0012,0.00064,0.0012,0.0012,1,1 +22890000,0.78,-0.015,0.00011,-0.63,0.0052,-0.0049,0.021,-0.0028,-0.0048,-3.7e+02,-0.0014,-0.0059,0.00015,0.0045,-0.065,-0.13,-0.11,-0.024,0.5,-0.00019,-0.091,-0.068,0,0,0.00036,0.00038,0.046,0.015,0.017,0.0072,0.055,0.057,0.036,4.8e-07,6.2e-07,4.1e-06,0.03,0.03,0.00025,0.0011,5.8e-05,0.0012,0.00064,0.0012,0.0012,1,1 +22990000,0.78,-0.015,0.00012,-0.63,0.0049,-0.0049,0.022,-0.0029,-0.0055,-3.7e+02,-0.0014,-0.0059,0.00016,0.0046,-0.065,-0.13,-0.11,-0.024,0.5,-0.00018,-0.091,-0.068,0,0,0.00036,0.00038,0.046,0.015,0.017,0.0071,0.058,0.06,0.036,4.7e-07,6e-07,4.1e-06,0.03,0.03,0.00025,0.0011,5.8e-05,0.0012,0.00064,0.0012,0.0012,1,1 +23090000,0.78,-0.015,0.00018,-0.63,0.0051,-0.0046,0.023,-0.0027,-0.0052,-3.7e+02,-0.0014,-0.0059,0.00015,0.0045,-0.065,-0.13,-0.11,-0.024,0.5,-0.00018,-0.091,-0.068,0,0,0.00036,0.00038,0.046,0.016,0.018,0.007,0.062,0.065,0.036,4.7e-07,6e-07,4.1e-06,0.03,0.03,0.00024,0.0011,5.8e-05,0.0012,0.00064,0.0012,0.0012,1,1 +23190000,0.78,-0.015,0.00017,-0.63,0.0027,-0.0034,0.024,-0.0055,-0.005,-3.7e+02,-0.0014,-0.0059,0.00016,0.0045,-0.065,-0.13,-0.11,-0.024,0.5,-0.00015,-0.091,-0.068,0,0,0.00036,0.00038,0.046,0.016,0.017,0.0069,0.065,0.067,0.035,4.6e-07,5.9e-07,4e-06,0.03,0.03,0.00024,0.0011,5.8e-05,0.0012,0.00064,0.0012,0.0012,1,1 +23290000,0.78,-0.015,0.00023,-0.63,0.0023,-0.0031,0.025,-0.0058,-0.0057,-3.7e+02,-0.0014,-0.0059,0.00016,0.0045,-0.065,-0.13,-0.11,-0.024,0.5,-0.00015,-0.091,-0.068,0,0,0.00036,0.00038,0.046,0.017,0.019,0.0069,0.07,0.073,0.036,4.6e-07,5.9e-07,4e-06,0.03,0.03,0.00023,0.0011,5.8e-05,0.0012,0.00064,0.0012,0.0012,1,1 +23390000,0.78,-0.015,0.00021,-0.63,-0.0011,-0.0028,0.022,-0.01,-0.0058,-3.7e+02,-0.0014,-0.0059,0.00016,0.0045,-0.065,-0.13,-0.11,-0.024,0.5,-0.00013,-0.091,-0.068,0,0,0.00036,0.00038,0.046,0.016,0.018,0.0068,0.072,0.075,0.035,4.5e-07,5.7e-07,4e-06,0.03,0.03,0.00023,0.0011,5.7e-05,0.0012,0.00064,0.0012,0.0012,1,1 +23490000,0.78,-0.013,-0.0019,-0.63,0.0044,-0.0021,-0.011,-0.011,-0.007,-3.7e+02,-0.0013,-0.0059,0.00017,0.0045,-0.065,-0.13,-0.11,-0.024,0.5,-0.00014,-0.091,-0.068,0,0,0.00036,0.00036,0.046,0.017,0.019,0.0068,0.078,0.081,0.035,4.5e-07,5.7e-07,4e-06,0.03,0.03,0.00023,0.0011,5.7e-05,0.0012,0.00064,0.0012,0.0012,1,1 +23590000,0.78,-0.0042,-0.0062,-0.63,0.015,0.0017,-0.043,-0.0099,-0.0038,-3.7e+02,-0.0013,-0.0059,0.00017,0.0045,-0.066,-0.13,-0.11,-0.024,0.5,-0.00012,-0.091,-0.068,0,0,0.00035,0.00034,0.046,0.014,0.016,0.0067,0.062,0.064,0.035,4.3e-07,5.5e-07,3.9e-06,0.03,0.03,0.00022,0.0011,5.7e-05,0.0012,0.00064,0.0012,0.0012,1,1 +23690000,0.78,0.0014,-0.0052,-0.63,0.042,0.016,-0.093,-0.0075,-0.0033,-3.7e+02,-0.0013,-0.0059,0.00018,0.0046,-0.066,-0.13,-0.11,-0.024,0.5,-0.00019,-0.091,-0.068,0,0,0.00034,0.00034,0.046,0.015,0.017,0.0067,0.067,0.069,0.035,4.3e-07,5.5e-07,3.9e-06,0.03,0.03,0.00022,0.0011,5.7e-05,0.0012,0.00064,0.0012,0.0012,1,1 +23790000,0.78,-0.0022,-0.0027,-0.63,0.063,0.033,-0.15,-0.0074,-0.0017,-3.7e+02,-0.0013,-0.0059,0.00018,0.0049,-0.066,-0.13,-0.11,-0.024,0.5,-0.00028,-0.09,-0.067,0,0,0.00034,0.00034,0.046,0.014,0.015,0.0066,0.055,0.056,0.035,4.2e-07,5.3e-07,3.9e-06,0.03,0.03,0.00022,0.0011,5.7e-05,0.0012,0.00064,0.0012,0.0012,1,1 +23890000,0.78,-0.0085,-0.0007,-0.63,0.077,0.045,-0.2,2.8e-05,0.0023,-3.7e+02,-0.0013,-0.0059,0.00019,0.005,-0.066,-0.13,-0.11,-0.024,0.5,-0.00033,-0.09,-0.067,0,0,0.00034,0.00035,0.046,0.014,0.016,0.0066,0.059,0.061,0.035,4.2e-07,5.3e-07,3.9e-06,0.03,0.03,0.00021,0.0011,5.7e-05,0.0012,0.00064,0.0012,0.0012,1,1 +23990000,0.78,-0.013,0.00021,-0.63,0.072,0.045,-0.25,-0.0055,0.00083,-3.7e+02,-0.0013,-0.0059,0.00018,0.0052,-0.067,-0.13,-0.11,-0.024,0.5,-0.00029,-0.09,-0.067,0,0,0.00035,0.00037,0.046,0.015,0.016,0.0066,0.062,0.063,0.035,4.1e-07,5.2e-07,3.9e-06,0.03,0.03,0.00021,0.0011,5.7e-05,0.0012,0.00064,0.0012,0.0012,1,1 +24090000,0.78,-0.012,-0.00093,-0.63,0.072,0.044,-0.3,0.00081,0.0045,-3.7e+02,-0.0013,-0.0059,0.00019,0.0053,-0.067,-0.13,-0.11,-0.024,0.5,-0.00034,-0.09,-0.067,0,0,0.00035,0.00036,0.046,0.015,0.017,0.0065,0.066,0.069,0.035,4.1e-07,5.2e-07,3.8e-06,0.03,0.03,0.00021,0.0011,5.7e-05,0.0012,0.00064,0.0012,0.0012,1,1 +24190000,0.78,-0.0097,-0.0017,-0.63,0.069,0.043,-0.35,-0.0065,0.0022,-3.7e+02,-0.0013,-0.0059,0.00018,0.0056,-0.068,-0.13,-0.11,-0.024,0.5,-0.00031,-0.09,-0.067,0,0,0.00035,0.00035,0.046,0.015,0.017,0.0065,0.069,0.071,0.034,4.1e-07,5.1e-07,3.8e-06,0.03,0.03,0.00021,0.0011,5.6e-05,0.0012,0.00063,0.0012,0.0012,1,1 +24290000,0.78,-0.0089,-0.0021,-0.63,0.077,0.048,-0.4,-0.00011,0.0069,-3.7e+02,-0.0013,-0.0059,0.00018,0.0056,-0.068,-0.13,-0.11,-0.024,0.5,-0.00033,-0.09,-0.067,0,0,0.00035,0.00035,0.046,0.016,0.018,0.0065,0.074,0.077,0.034,4e-07,5.1e-07,3.8e-06,0.03,0.03,0.0002,0.0011,5.6e-05,0.0012,0.00063,0.0012,0.0012,1,1 +24390000,0.78,-0.0093,-0.0022,-0.63,0.074,0.047,-0.46,-0.013,0.00027,-3.7e+02,-0.0012,-0.0059,0.00015,0.0062,-0.069,-0.13,-0.11,-0.024,0.5,-0.0003,-0.089,-0.068,0,0,0.00035,0.00035,0.046,0.016,0.018,0.0064,0.076,0.079,0.034,4e-07,5e-07,3.8e-06,0.03,0.03,0.0002,0.0011,5.6e-05,0.0012,0.00063,0.0012,0.0012,1,1 +24490000,0.78,-0.0051,-0.0026,-0.63,0.085,0.054,-0.51,-0.0046,0.0052,-3.7e+02,-0.0012,-0.0059,0.00015,0.0063,-0.069,-0.13,-0.11,-0.024,0.5,-0.00033,-0.089,-0.068,0,0,0.00034,0.00034,0.046,0.017,0.02,0.0064,0.082,0.085,0.034,4e-07,5e-07,3.8e-06,0.03,0.03,0.0002,0.0011,5.6e-05,0.0012,0.00063,0.0012,0.0012,1,1 +24590000,0.78,-0.0016,-0.0027,-0.63,0.089,0.057,-0.56,-0.018,-0.0039,-3.7e+02,-0.0012,-0.0059,0.00014,0.0069,-0.071,-0.13,-0.11,-0.024,0.5,-0.00037,-0.089,-0.068,0,0,0.00034,0.00034,0.046,0.017,0.019,0.0063,0.084,0.088,0.034,3.9e-07,4.9e-07,3.7e-06,0.03,0.03,0.0002,0.0011,5.6e-05,0.0012,0.00063,0.0012,0.0012,1,1 +24690000,0.78,-0.00074,-0.0027,-0.63,0.11,0.073,-0.64,-0.0089,0.0014,-3.7e+02,-0.0012,-0.0059,0.00015,0.0072,-0.071,-0.13,-0.11,-0.024,0.5,-0.00055,-0.089,-0.068,0,0,0.00034,0.00034,0.045,0.018,0.021,0.0063,0.09,0.094,0.034,3.9e-07,4.9e-07,3.7e-06,0.03,0.03,0.00019,0.0011,5.6e-05,0.0012,0.00063,0.0012,0.0012,1,1 +24790000,0.78,-0.0023,-0.0024,-0.63,0.11,0.082,-0.73,-0.028,-0.0034,-3.7e+02,-0.0012,-0.0059,0.00013,0.0079,-0.073,-0.13,-0.11,-0.025,0.5,-0.00036,-0.088,-0.068,0,0,0.00034,0.00034,0.045,0.018,0.021,0.0062,0.093,0.096,0.034,3.8e-07,4.8e-07,3.7e-06,0.03,0.03,0.00019,0.0011,5.6e-05,0.0012,0.00062,0.0012,0.0012,1,1 +24890000,0.78,-0.00046,-0.004,-0.63,0.13,0.097,-0.75,-0.017,0.0055,-3.7e+02,-0.0012,-0.0059,0.00012,0.0081,-0.074,-0.13,-0.11,-0.025,0.5,-0.00045,-0.088,-0.068,0,0,0.00034,0.00034,0.045,0.019,0.022,0.0062,0.099,0.1,0.034,3.8e-07,4.8e-07,3.7e-06,0.03,0.03,0.00019,0.0011,5.5e-05,0.0012,0.00062,0.0012,0.0012,1,1 +24990000,0.78,0.0012,-0.0055,-0.63,0.13,0.1,-0.81,-0.039,-0.001,-3.7e+02,-0.0011,-0.0059,9e-05,0.0092,-0.076,-0.13,-0.11,-0.025,0.5,-0.00024,-0.087,-0.069,0,0,0.00034,0.00034,0.045,0.019,0.022,0.0062,0.1,0.11,0.034,3.8e-07,4.7e-07,3.7e-06,0.03,0.03,0.00019,0.0011,5.5e-05,0.0012,0.00061,0.0012,0.0012,1,1 +25090000,0.78,0.00063,-0.0059,-0.63,0.16,0.12,-0.86,-0.025,0.011,-3.7e+02,-0.0011,-0.0059,8.4e-05,0.0094,-0.076,-0.13,-0.11,-0.025,0.5,-0.00025,-0.087,-0.068,0,0,0.00034,0.00034,0.044,0.02,0.023,0.0062,0.11,0.11,0.034,3.8e-07,4.7e-07,3.7e-06,0.03,0.03,0.00019,0.0011,5.5e-05,0.0012,0.00061,0.0011,0.0012,1,1 +25190000,0.78,-0.0015,-0.0054,-0.62,0.15,0.11,-0.91,-0.069,-0.012,-3.7e+02,-0.0011,-0.0059,4.4e-05,0.011,-0.081,-0.13,-0.11,-0.025,0.5,-0.00022,-0.086,-0.069,0,0,0.00034,0.00033,0.044,0.02,0.023,0.0061,0.11,0.11,0.033,3.7e-07,4.6e-07,3.6e-06,0.03,0.03,0.00018,0.0011,5.5e-05,0.0012,0.0006,0.0011,0.0012,1,1 +25290000,0.78,0.0055,-0.0066,-0.62,0.17,0.13,-0.96,-0.053,-0.00075,-3.7e+02,-0.0011,-0.0059,4.8e-05,0.011,-0.081,-0.13,-0.11,-0.025,0.5,-0.00034,-0.086,-0.069,0,0,0.00033,0.00034,0.044,0.021,0.024,0.0061,0.12,0.12,0.033,3.7e-07,4.6e-07,3.6e-06,0.03,0.03,0.00018,0.0011,5.4e-05,0.0012,0.0006,0.0011,0.0012,1,1 +25390000,0.78,0.011,-0.0071,-0.62,0.18,0.13,-1,-0.1,-0.025,-3.7e+02,-0.001,-0.0058,8.1e-06,0.014,-0.086,-0.13,-0.11,-0.025,0.5,-0.00037,-0.085,-0.069,0,0,0.00033,0.00036,0.043,0.021,0.024,0.0061,0.12,0.12,0.033,3.6e-07,4.5e-07,3.6e-06,0.03,0.03,0.00018,0.0011,5.4e-05,0.0012,0.00059,0.0011,0.0012,1,1 +25490000,0.78,0.013,-0.0072,-0.63,0.22,0.16,-1.1,-0.082,-0.012,-3.7e+02,-0.001,-0.0058,2.3e-05,0.014,-0.087,-0.13,-0.11,-0.025,0.5,-0.00072,-0.084,-0.069,0,0,0.00033,0.00036,0.042,0.022,0.026,0.0061,0.13,0.13,0.033,3.6e-07,4.5e-07,3.6e-06,0.03,0.03,0.00018,0.0011,5.3e-05,0.0012,0.00058,0.0011,0.0012,1,1 +25590000,0.78,0.011,-0.007,-0.63,0.25,0.19,-1.1,-0.059,0.0041,-3.7e+02,-0.001,-0.0058,3.2e-05,0.014,-0.087,-0.13,-0.12,-0.025,0.5,-0.00098,-0.084,-0.068,0,0,0.00033,0.00035,0.042,0.024,0.028,0.0061,0.14,0.14,0.033,3.6e-07,4.5e-07,3.6e-06,0.03,0.03,0.00018,0.0011,5.3e-05,0.0011,0.00058,0.0011,0.0011,1,1 +25690000,0.78,0.018,-0.0099,-0.63,0.29,0.21,-1.2,-0.032,0.022,-3.7e+02,-0.001,-0.0058,4.3e-05,0.015,-0.088,-0.13,-0.12,-0.026,0.5,-0.0013,-0.082,-0.068,0,0,0.00034,0.00039,0.042,0.025,0.03,0.0061,0.14,0.15,0.033,3.6e-07,4.5e-07,3.6e-06,0.03,0.03,0.00017,0.001,5.2e-05,0.0011,0.00057,0.0011,0.0011,1,1 +25790000,0.78,0.024,-0.012,-0.63,0.35,0.25,-1.2,-0.00023,0.043,-3.7e+02,-0.00099,-0.0058,6.3e-05,0.015,-0.088,-0.13,-0.12,-0.026,0.5,-0.0019,-0.081,-0.067,0,0,0.00034,0.00043,0.041,0.027,0.033,0.0061,0.15,0.16,0.033,3.6e-07,4.5e-07,3.5e-06,0.03,0.03,0.00017,0.001,5.1e-05,0.0011,0.00057,0.0011,0.0011,1,1 +25890000,0.77,0.025,-0.012,-0.63,0.41,0.28,-1.3,0.039,0.066,-3.7e+02,-0.00099,-0.0058,8.6e-05,0.015,-0.089,-0.13,-0.12,-0.026,0.5,-0.0025,-0.079,-0.066,0,0,0.00034,0.00043,0.04,0.029,0.037,0.0061,0.17,0.17,0.033,3.6e-07,4.5e-07,3.5e-06,0.03,0.03,0.00017,0.001,5e-05,0.0011,0.00056,0.0011,0.0011,1,1 +25990000,0.77,0.021,-0.012,-0.63,0.47,0.31,-1.3,0.083,0.093,-3.7e+02,-0.00099,-0.0058,9.9e-05,0.015,-0.089,-0.13,-0.12,-0.027,0.5,-0.0029,-0.078,-0.065,0,0,0.00034,0.00041,0.04,0.032,0.04,0.0061,0.18,0.19,0.033,3.6e-07,4.5e-07,3.5e-06,0.03,0.03,0.00017,0.00099,5e-05,0.0011,0.00055,0.001,0.0011,1,1 +26090000,0.78,0.032,-0.016,-0.63,0.52,0.35,-1.3,0.13,0.13,-3.7e+02,-0.00098,-0.0058,8.8e-05,0.016,-0.089,-0.13,-0.12,-0.027,0.5,-0.0028,-0.077,-0.065,0,0,0.00035,0.00049,0.039,0.034,0.043,0.0061,0.19,0.2,0.033,3.6e-07,4.5e-07,3.5e-06,0.03,0.03,0.00017,0.00097,4.8e-05,0.0011,0.00054,0.001,0.0011,1,1 +26190000,0.78,0.041,-0.017,-0.63,0.6,0.4,-1.3,0.19,0.16,-3.7e+02,-0.00098,-0.0058,9.6e-05,0.018,-0.09,-0.13,-0.13,-0.028,0.5,-0.0035,-0.074,-0.063,0,0,0.00036,0.00058,0.037,0.036,0.047,0.0061,0.2,0.22,0.033,3.6e-07,4.5e-07,3.5e-06,0.03,0.03,0.00017,0.00093,4.7e-05,0.001,0.00053,0.00098,0.001,1,1 +26290000,0.78,0.044,-0.018,-0.63,0.68,0.45,-1.3,0.25,0.2,-3.7e+02,-0.00097,-0.0058,9.2e-05,0.019,-0.091,-0.13,-0.13,-0.028,0.49,-0.0037,-0.071,-0.061,0,0,0.00036,0.0006,0.036,0.039,0.052,0.0061,0.21,0.23,0.033,3.6e-07,4.5e-07,3.5e-06,0.03,0.03,0.00017,0.00091,4.6e-05,0.001,0.00052,0.00094,0.001,1,1 +26390000,0.77,0.04,-0.018,-0.63,0.76,0.5,-1.3,0.32,0.25,-3.7e+02,-0.00097,-0.0058,0.0001,0.02,-0.091,-0.13,-0.13,-0.028,0.49,-0.0042,-0.069,-0.06,0,0,0.00036,0.00055,0.034,0.042,0.056,0.0061,0.23,0.25,0.033,3.6e-07,4.5e-07,3.4e-06,0.03,0.03,0.00016,0.00088,4.4e-05,0.00098,0.0005,0.00091,0.00097,1,1 +26490000,0.77,0.056,-0.024,-0.63,0.84,0.55,-1.3,0.4,0.3,-3.7e+02,-0.00096,-0.0058,0.0001,0.02,-0.092,-0.13,-0.13,-0.029,0.49,-0.0043,-0.067,-0.058,0,0,0.00038,0.00075,0.033,0.044,0.061,0.0061,0.24,0.27,0.033,3.6e-07,4.5e-07,3.4e-06,0.03,0.03,0.00016,0.00084,4.2e-05,0.00094,0.00048,0.00088,0.00094,1,1 +26590000,0.77,0.073,-0.029,-0.63,0.95,0.63,-1.3,0.48,0.36,-3.7e+02,-0.00096,-0.0058,7.4e-05,0.023,-0.092,-0.13,-0.13,-0.03,0.49,-0.0039,-0.064,-0.057,0,0,0.00041,0.00098,0.031,0.048,0.067,0.0061,0.26,0.29,0.033,3.6e-07,4.5e-07,3.4e-06,0.03,0.03,0.00016,0.0008,4e-05,0.00089,0.00046,0.00083,0.00089,1,1 +26690000,0.77,0.076,-0.03,-0.64,1.1,0.71,-1.3,0.59,0.42,-3.7e+02,-0.00096,-0.0058,8.5e-05,0.024,-0.093,-0.13,-0.14,-0.031,0.49,-0.0049,-0.059,-0.052,0,0,0.00041,0.00097,0.028,0.052,0.073,0.0061,0.28,0.31,0.033,3.6e-07,4.5e-07,3.4e-06,0.03,0.03,0.00016,0.00074,3.8e-05,0.00083,0.00044,0.00077,0.00083,1,1 +26790000,0.77,0.07,-0.029,-0.64,1.2,0.79,-1.3,0.7,0.5,-3.7e+02,-0.00095,-0.0058,6.9e-05,0.025,-0.093,-0.13,-0.14,-0.032,0.48,-0.0047,-0.055,-0.049,0,0,0.00039,0.00084,0.026,0.055,0.079,0.0061,0.3,0.33,0.033,3.7e-07,4.5e-07,3.4e-06,0.03,0.03,0.00016,0.0007,3.6e-05,0.00079,0.00041,0.00073,0.00078,1,1 +26890000,0.76,0.093,-0.036,-0.64,1.3,0.86,-1.3,0.83,0.58,-3.7e+02,-0.00095,-0.0058,7.2e-05,0.026,-0.093,-0.13,-0.15,-0.032,0.48,-0.0052,-0.052,-0.047,0,0,0.00044,0.0011,0.024,0.058,0.085,0.0061,0.32,0.35,0.033,3.7e-07,4.6e-07,3.3e-06,0.03,0.03,0.00016,0.00066,3.4e-05,0.00074,0.00039,0.00068,0.00074,1,1 +26990000,0.76,0.12,-0.041,-0.64,1.5,0.97,-1.3,0.98,0.67,-3.7e+02,-0.00095,-0.0058,6.1e-05,0.029,-0.094,-0.13,-0.15,-0.034,0.48,-0.0055,-0.046,-0.043,0,0,0.00049,0.0014,0.021,0.061,0.091,0.0061,0.34,0.37,0.033,3.7e-07,4.6e-07,3.3e-06,0.03,0.03,0.00016,0.0006,3.1e-05,0.00067,0.00036,0.00062,0.00067,1,1 +27090000,0.76,0.12,-0.041,-0.64,1.7,1.1,-1.2,1.1,0.78,-3.7e+02,-0.00095,-0.0058,4.5e-05,0.03,-0.093,-0.13,-0.16,-0.035,0.47,-0.0055,-0.041,-0.038,0,0,0.00049,0.0013,0.018,0.065,0.098,0.0061,0.36,0.4,0.033,3.7e-07,4.6e-07,3.3e-06,0.03,0.03,0.00016,0.00055,2.8e-05,0.0006,0.00033,0.00056,0.0006,1,1 +27190000,0.76,0.11,-0.039,-0.64,1.9,1.2,-1.2,1.3,0.9,-3.7e+02,-0.00096,-0.0058,2.8e-05,0.032,-0.092,-0.13,-0.16,-0.035,0.47,-0.0053,-0.038,-0.035,0,0,0.00045,0.0011,0.017,0.068,0.1,0.0062,0.38,0.43,0.034,3.7e-07,4.6e-07,3.3e-06,0.03,0.03,0.00015,0.00051,2.6e-05,0.00056,0.00031,0.00052,0.00056,1,1 +27290000,0.76,0.093,-0.035,-0.64,2,1.3,-1.2,1.5,1,-3.7e+02,-0.00096,-0.0058,2.2e-05,0.032,-0.091,-0.13,-0.16,-0.036,0.47,-0.0054,-0.035,-0.033,0,0,0.00041,0.00083,0.015,0.07,0.11,0.0062,0.4,0.46,0.033,3.7e-07,4.6e-07,3.3e-06,0.03,0.03,0.00015,0.00048,2.5e-05,0.00053,0.00029,0.00049,0.00052,1,1 +27390000,0.76,0.077,-0.03,-0.64,2.1,1.4,-1.2,1.7,1.2,-3.7e+02,-0.00095,-0.0058,1.7e-05,0.033,-0.089,-0.13,-0.17,-0.036,0.47,-0.0054,-0.033,-0.032,0,0,0.00038,0.00064,0.014,0.071,0.11,0.0062,0.43,0.49,0.033,3.7e-07,4.6e-07,3.3e-06,0.029,0.03,0.00015,0.00046,2.4e-05,0.00051,0.00027,0.00047,0.00051,1,1 +27490000,0.76,0.061,-0.025,-0.64,2.2,1.4,-1.2,1.9,1.3,-3.7e+02,-0.00095,-0.0058,9e-06,0.033,-0.087,-0.13,-0.17,-0.037,0.47,-0.0052,-0.032,-0.032,0,0,0.00036,0.00052,0.012,0.072,0.11,0.0062,0.46,0.52,0.033,3.7e-07,4.6e-07,3.3e-06,0.029,0.03,0.00015,0.00044,2.3e-05,0.0005,0.00025,0.00046,0.0005,1,1 +27590000,0.77,0.049,-0.022,-0.64,2.3,1.5,-1.2,2.2,1.5,-3.7e+02,-0.00095,-0.0058,-8.8e-07,0.034,-0.084,-0.13,-0.17,-0.037,0.47,-0.0048,-0.031,-0.031,0,0,0.00035,0.00045,0.012,0.073,0.11,0.0062,0.48,0.55,0.033,3.7e-07,4.6e-07,3.3e-06,0.029,0.03,0.00015,0.00043,2.3e-05,0.00049,0.00024,0.00045,0.00049,1,1 +27690000,0.77,0.047,-0.021,-0.64,2.3,1.5,-1.2,2.4,1.6,-3.7e+02,-0.00095,-0.0058,-9.9e-06,0.034,-0.083,-0.13,-0.17,-0.037,0.47,-0.0044,-0.031,-0.031,0,0,0.00035,0.00044,0.011,0.073,0.11,0.0063,0.51,0.59,0.033,3.7e-07,4.6e-07,3.3e-06,0.029,0.03,0.00015,0.00043,2.2e-05,0.00049,0.00023,0.00045,0.00049,1,1 +27790000,0.77,0.049,-0.021,-0.64,2.3,1.5,-1.2,2.6,1.8,-3.7e+02,-0.00095,-0.0058,-2.3e-05,0.035,-0.081,-0.13,-0.17,-0.037,0.47,-0.0039,-0.03,-0.031,0,0,0.00035,0.00044,0.0098,0.074,0.1,0.0063,0.54,0.62,0.033,3.7e-07,4.6e-07,3.3e-06,0.029,0.03,0.00015,0.00042,2.2e-05,0.00049,0.00022,0.00044,0.00048,1,1 +27890000,0.77,0.047,-0.021,-0.64,2.4,1.6,-1.2,2.8,1.9,-3.7e+02,-0.00095,-0.0058,-2.4e-05,0.034,-0.079,-0.13,-0.17,-0.037,0.47,-0.0039,-0.03,-0.03,0,0,0.00035,0.00043,0.0093,0.075,0.1,0.0063,0.58,0.66,0.034,3.7e-07,4.6e-07,3.3e-06,0.029,0.03,0.00015,0.00041,2.2e-05,0.00048,0.00022,0.00044,0.00048,1,1 +27990000,0.77,0.043,-0.02,-0.64,2.4,1.6,-1.2,3.1,2.1,-3.7e+02,-0.00095,-0.0058,-2.7e-05,0.034,-0.078,-0.13,-0.17,-0.037,0.47,-0.0039,-0.03,-0.03,0,0,0.00035,0.00041,0.0087,0.076,0.1,0.0064,0.61,0.7,0.033,3.7e-07,4.7e-07,3.3e-06,0.029,0.03,0.00014,0.00041,2.1e-05,0.00048,0.00021,0.00043,0.00048,1,1 +28090000,0.77,0.057,-0.025,-0.64,2.4,1.6,-1.2,3.3,2.3,-3.7e+02,-0.00095,-0.0058,-3.8e-05,0.035,-0.076,-0.13,-0.17,-0.038,0.47,-0.0035,-0.029,-0.03,0,0,0.00035,0.00045,0.0081,0.077,0.1,0.0064,0.65,0.74,0.033,3.7e-07,4.7e-07,3.3e-06,0.029,0.03,0.00014,0.0004,2.1e-05,0.00048,0.00021,0.00042,0.00048,1,1 +28190000,0.77,0.071,-0.028,-0.63,2.5,1.6,-0.93,3.6,2.4,-3.7e+02,-0.00095,-0.0058,-4e-05,0.035,-0.074,-0.13,-0.17,-0.038,0.46,-0.0035,-0.029,-0.03,0,0,0.00036,0.00049,0.0077,0.078,0.1,0.0065,0.68,0.79,0.034,3.7e-07,4.7e-07,3.3e-06,0.029,0.03,0.00014,0.00039,2.1e-05,0.00047,0.0002,0.00042,0.00047,1,1 +28290000,0.77,0.053,-0.022,-0.64,2.5,1.7,-0.065,3.8,2.6,-3.7e+02,-0.00095,-0.0058,-4.9e-05,0.035,-0.071,-0.13,-0.17,-0.038,0.46,-0.0033,-0.028,-0.029,0,0,0.00035,0.00043,0.0074,0.077,0.1,0.0066,0.72,0.83,0.034,3.7e-07,4.7e-07,3.3e-06,0.029,0.03,0.00014,0.00038,2e-05,0.00046,0.0002,0.00041,0.00046,1,1 +28390000,0.77,0.02,-0.0094,-0.64,2.5,1.7,0.79,4,2.8,-3.7e+02,-0.00095,-0.0058,-5.7e-05,0.035,-0.068,-0.13,-0.17,-0.038,0.46,-0.0031,-0.027,-0.029,0,0,0.00035,0.00037,0.0071,0.076,0.1,0.0066,0.76,0.88,0.034,3.7e-07,4.7e-07,3.3e-06,0.029,0.03,0.00014,0.00038,2e-05,0.00046,0.00019,0.00041,0.00046,1,1 +28490000,0.77,0.0014,-0.0026,-0.64,2.4,1.7,1.1,4.3,3,-3.7e+02,-0.00096,-0.0058,-6.3e-05,0.035,-0.065,-0.13,-0.17,-0.038,0.46,-0.0031,-0.027,-0.029,0,0,0.00034,0.00036,0.0068,0.077,0.099,0.0067,0.8,0.93,0.034,3.7e-07,4.7e-07,3.3e-06,0.029,0.03,0.00014,0.00038,2e-05,0.00046,0.00019,0.00041,0.00046,1,1 +28590000,0.77,-0.0022,-0.0011,-0.64,2.4,1.6,0.99,4.5,3.1,-3.7e+02,-0.00096,-0.0058,-6.4e-05,0.034,-0.064,-0.12,-0.17,-0.038,0.46,-0.0031,-0.027,-0.029,0,0,0.00034,0.00036,0.0065,0.077,0.098,0.0067,0.84,0.98,0.034,3.7e-07,4.7e-07,3.3e-06,0.029,0.029,0.00014,0.00038,2e-05,0.00046,0.00019,0.00041,0.00046,1,1 +28690000,0.77,-0.0032,-0.00057,-0.64,2.3,1.6,0.99,4.8,3.3,-3.7e+02,-0.00097,-0.0058,-7.1e-05,0.034,-0.064,-0.12,-0.17,-0.038,0.46,-0.003,-0.027,-0.029,0,0,0.00034,0.00036,0.0063,0.078,0.098,0.0067,0.88,1,0.034,3.6e-07,4.7e-07,3.3e-06,0.029,0.029,0.00014,0.00038,2e-05,0.00046,0.00018,0.0004,0.00046,1,1 +28790000,0.77,-0.0034,-0.00035,-0.63,2.2,1.6,1,5,3.4,-3.7e+02,-0.00098,-0.0058,-7.9e-05,0.034,-0.061,-0.12,-0.17,-0.038,0.46,-0.0029,-0.027,-0.029,0,0,0.00034,0.00037,0.0061,0.079,0.098,0.0068,0.92,1.1,0.034,3.6e-07,4.7e-07,3.3e-06,0.029,0.029,0.00014,0.00038,2e-05,0.00045,0.00018,0.0004,0.00045,1,1 +28890000,0.77,-0.0032,-0.00036,-0.63,2.2,1.5,0.99,5.2,3.6,-3.7e+02,-0.00099,-0.0058,-8.6e-05,0.033,-0.06,-0.12,-0.17,-0.038,0.46,-0.0028,-0.027,-0.029,0,0,0.00034,0.00037,0.0059,0.08,0.1,0.0068,0.97,1.1,0.034,3.6e-07,4.7e-07,3.3e-06,0.029,0.029,0.00013,0.00038,2e-05,0.00045,0.00018,0.0004,0.00045,1,1 +28990000,0.77,-0.0027,-0.00054,-0.63,2.1,1.5,0.98,5.5,3.8,-3.7e+02,-0.001,-0.0058,-9.8e-05,0.033,-0.058,-0.12,-0.17,-0.038,0.46,-0.0026,-0.027,-0.028,0,0,0.00034,0.00037,0.0057,0.081,0.1,0.0069,1,1.2,0.034,3.6e-07,4.8e-07,3.3e-06,0.029,0.029,0.00013,0.00038,2e-05,0.00045,0.00018,0.0004,0.00045,1,1 +29090000,0.78,-0.0022,-0.0007,-0.63,2.1,1.5,0.97,5.7,3.9,-3.7e+02,-0.001,-0.0058,-0.0001,0.032,-0.056,-0.12,-0.17,-0.038,0.46,-0.0025,-0.027,-0.028,0,0,0.00034,0.00037,0.0055,0.083,0.1,0.0069,1.1,1.3,0.034,3.5e-07,4.8e-07,3.3e-06,0.029,0.029,0.00013,0.00037,2e-05,0.00045,0.00018,0.0004,0.00045,1,1 +29190000,0.77,-0.0019,-0.00078,-0.63,2,1.5,0.97,5.9,4.1,-3.7e+02,-0.001,-0.0058,-0.0001,0.032,-0.055,-0.12,-0.17,-0.038,0.46,-0.0025,-0.028,-0.028,0,0,0.00034,0.00037,0.0054,0.084,0.1,0.007,1.1,1.3,0.034,3.5e-07,4.8e-07,3.3e-06,0.029,0.028,0.00013,0.00037,2e-05,0.00045,0.00017,0.0004,0.00045,1,1 +29290000,0.78,-0.00093,-0.0011,-0.63,1.9,1.4,1,6.1,4.2,-3.7e+02,-0.001,-0.0058,-0.00011,0.031,-0.053,-0.12,-0.17,-0.038,0.46,-0.0024,-0.028,-0.028,0,0,0.00033,0.00037,0.0053,0.086,0.11,0.007,1.2,1.4,0.034,3.5e-07,4.8e-07,3.3e-06,0.029,0.028,0.00013,0.00037,2e-05,0.00045,0.00017,0.0004,0.00044,1,1 +29390000,0.78,0.00051,-0.0014,-0.63,1.9,1.4,1,6.3,4.4,-3.7e+02,-0.001,-0.0058,-0.00012,0.03,-0.051,-0.12,-0.17,-0.038,0.46,-0.0021,-0.027,-0.028,0,0,0.00033,0.00037,0.0051,0.087,0.11,0.007,1.2,1.4,0.034,3.5e-07,4.8e-07,3.3e-06,0.029,0.028,0.00013,0.00037,2e-05,0.00044,0.00017,0.0004,0.00044,1,1 +29490000,0.78,0.0017,-0.0018,-0.63,1.8,1.4,1,6.4,4.5,-3.7e+02,-0.001,-0.0058,-0.00013,0.03,-0.05,-0.12,-0.17,-0.038,0.46,-0.0021,-0.027,-0.028,0,0,0.00033,0.00037,0.005,0.089,0.11,0.0071,1.3,1.5,0.034,3.5e-07,4.8e-07,3.3e-06,0.029,0.028,0.00013,0.00037,2e-05,0.00044,0.00017,0.0004,0.00044,1,1 +29590000,0.78,0.0028,-0.002,-0.63,1.8,1.4,1,6.6,4.7,-3.7e+02,-0.001,-0.0058,-0.00013,0.028,-0.048,-0.12,-0.17,-0.038,0.46,-0.002,-0.028,-0.028,0,0,0.00033,0.00037,0.0049,0.091,0.12,0.0071,1.3,1.6,0.034,3.4e-07,4.8e-07,3.2e-06,0.029,0.028,0.00013,0.00037,2e-05,0.00044,0.00017,0.0004,0.00044,1,1 +29690000,0.78,0.0036,-0.0023,-0.63,1.8,1.4,0.99,6.8,4.8,-3.7e+02,-0.001,-0.0058,-0.00014,0.028,-0.045,-0.12,-0.17,-0.038,0.46,-0.0019,-0.028,-0.028,0,0,0.00033,0.00037,0.0049,0.093,0.12,0.0071,1.4,1.7,0.034,3.4e-07,4.8e-07,3.2e-06,0.029,0.028,0.00012,0.00037,2e-05,0.00044,0.00017,0.0004,0.00044,1,1 +29790000,0.78,0.0041,-0.0025,-0.63,1.7,1.3,0.98,7,4.9,-3.7e+02,-0.001,-0.0058,-0.00014,0.027,-0.042,-0.12,-0.17,-0.038,0.46,-0.0019,-0.028,-0.028,0,0,0.00033,0.00037,0.0048,0.095,0.12,0.0071,1.4,1.7,0.034,3.4e-07,4.8e-07,3.2e-06,0.029,0.028,0.00012,0.00037,2e-05,0.00044,0.00017,0.0004,0.00044,1,1 +29890000,0.78,0.0045,-0.0026,-0.63,1.7,1.3,0.97,7.2,5.1,-3.7e+02,-0.001,-0.0058,-0.00015,0.026,-0.038,-0.12,-0.17,-0.038,0.46,-0.0017,-0.028,-0.028,0,0,0.00033,0.00038,0.0047,0.097,0.13,0.0072,1.5,1.8,0.034,3.4e-07,4.8e-07,3.2e-06,0.029,0.027,0.00012,0.00037,1.9e-05,0.00044,0.00017,0.0004,0.00044,1,1 +29990000,0.78,0.0046,-0.0027,-0.63,1.7,1.3,0.95,7.3,5.2,-3.7e+02,-0.001,-0.0058,-0.00015,0.025,-0.035,-0.12,-0.17,-0.038,0.46,-0.0016,-0.028,-0.028,0,0,0.00033,0.00038,0.0046,0.099,0.13,0.0072,1.6,1.9,0.034,3.4e-07,4.8e-07,3.2e-06,0.029,0.027,0.00012,0.00037,1.9e-05,0.00044,0.00017,0.0004,0.00044,1,1 +30090000,0.78,0.0046,-0.0027,-0.63,1.6,1.3,0.94,7.5,5.3,-3.7e+02,-0.001,-0.0058,-0.00016,0.024,-0.033,-0.12,-0.17,-0.038,0.46,-0.0015,-0.028,-0.028,0,0,0.00032,0.00038,0.0046,0.1,0.14,0.0072,1.6,2,0.034,3.3e-07,4.9e-07,3.2e-06,0.029,0.027,0.00012,0.00037,1.9e-05,0.00044,0.00016,0.0004,0.00043,1,1 +30190000,0.78,0.0043,-0.0026,-0.63,1.6,1.3,0.93,7.7,5.5,-3.7e+02,-0.001,-0.0058,-0.00016,0.023,-0.033,-0.12,-0.17,-0.038,0.46,-0.0015,-0.028,-0.028,0,0,0.00032,0.00038,0.0045,0.1,0.14,0.0072,1.7,2.1,0.035,3.3e-07,4.9e-07,3.2e-06,0.029,0.027,0.00012,0.00037,1.9e-05,0.00044,0.00016,0.0004,0.00043,1,1 +30290000,0.78,0.0042,-0.0026,-0.63,1.5,1.3,0.92,7.8,5.6,-3.7e+02,-0.0011,-0.0058,-0.00016,0.022,-0.031,-0.12,-0.17,-0.038,0.46,-0.0014,-0.028,-0.028,0,0,0.00032,0.00038,0.0044,0.11,0.15,0.0072,1.8,2.2,0.035,3.3e-07,4.9e-07,3.2e-06,0.029,0.027,0.00012,0.00037,1.9e-05,0.00044,0.00016,0.0004,0.00043,1,1 +30390000,0.78,0.0041,-0.0026,-0.63,1.5,1.2,0.9,8,5.7,-3.7e+02,-0.0011,-0.0058,-0.00017,0.021,-0.028,-0.12,-0.17,-0.038,0.46,-0.0014,-0.028,-0.027,0,0,0.00032,0.00038,0.0044,0.11,0.15,0.0072,1.8,2.3,0.035,3.3e-07,4.9e-07,3.2e-06,0.029,0.027,0.00012,0.00037,1.9e-05,0.00043,0.00016,0.0004,0.00043,1,1 +30490000,0.78,0.0039,-0.0025,-0.63,1.5,1.2,0.89,8.2,5.8,-3.7e+02,-0.0011,-0.0058,-0.00017,0.02,-0.026,-0.12,-0.17,-0.038,0.46,-0.0013,-0.028,-0.027,0,0,0.00032,0.00038,0.0044,0.11,0.16,0.0072,1.9,2.4,0.035,3.3e-07,4.9e-07,3.2e-06,0.029,0.026,0.00012,0.00037,1.9e-05,0.00043,0.00016,0.0004,0.00043,1,1 +30590000,0.78,0.0036,-0.0025,-0.63,1.5,1.2,0.85,8.3,6,-3.7e+02,-0.0011,-0.0058,-0.00017,0.019,-0.022,-0.12,-0.17,-0.038,0.46,-0.0013,-0.028,-0.027,0,0,0.00032,0.00038,0.0043,0.11,0.16,0.0072,2,2.5,0.035,3.3e-07,4.9e-07,3.2e-06,0.029,0.026,0.00011,0.00037,1.9e-05,0.00043,0.00016,0.0004,0.00043,1,1 +30690000,0.78,0.0034,-0.0024,-0.63,1.4,1.2,0.84,8.5,6.1,-3.7e+02,-0.0011,-0.0058,-0.00017,0.018,-0.019,-0.12,-0.17,-0.038,0.46,-0.0013,-0.028,-0.027,0,0,0.00032,0.00039,0.0043,0.11,0.17,0.0072,2.1,2.6,0.035,3.2e-07,4.9e-07,3.2e-06,0.029,0.026,0.00011,0.00036,1.9e-05,0.00043,0.00016,0.0004,0.00043,1,1 +30790000,0.78,0.0031,-0.0023,-0.63,1.4,1.2,0.83,8.6,6.2,-3.7e+02,-0.0011,-0.0058,-0.00017,0.017,-0.018,-0.12,-0.17,-0.038,0.46,-0.0012,-0.028,-0.027,0,0,0.00031,0.00039,0.0042,0.12,0.18,0.0072,2.2,2.8,0.035,3.2e-07,4.9e-07,3.2e-06,0.029,0.026,0.00011,0.00036,1.9e-05,0.00043,0.00016,0.0004,0.00043,1,1 +30890000,0.78,0.0027,-0.0022,-0.63,1.4,1.2,0.82,8.8,6.3,-3.7e+02,-0.0011,-0.0058,-0.00018,0.016,-0.016,-0.12,-0.17,-0.038,0.46,-0.0011,-0.028,-0.027,0,0,0.00031,0.00039,0.0042,0.12,0.18,0.0072,2.2,2.9,0.035,3.2e-07,4.9e-07,3.2e-06,0.029,0.026,0.00011,0.00036,1.9e-05,0.00043,0.00016,0.0004,0.00043,1,1 +30990000,0.78,0.0023,-0.0022,-0.63,1.3,1.2,0.81,8.9,6.4,-3.7e+02,-0.0011,-0.0058,-0.00018,0.015,-0.012,-0.12,-0.17,-0.038,0.46,-0.0011,-0.028,-0.027,0,0,0.00031,0.00039,0.0042,0.12,0.19,0.0072,2.3,3,0.035,3.2e-07,4.9e-07,3.2e-06,0.029,0.026,0.00011,0.00036,1.9e-05,0.00043,0.00016,0.0004,0.00043,1,1 +31090000,0.78,0.0019,-0.0021,-0.63,1.3,1.1,0.8,9,6.6,-3.7e+02,-0.0011,-0.0058,-0.00018,0.014,-0.0094,-0.12,-0.17,-0.038,0.46,-0.00097,-0.029,-0.027,0,0,0.00031,0.00039,0.0041,0.12,0.2,0.0072,2.4,3.2,0.035,3.2e-07,4.9e-07,3.2e-06,0.029,0.025,0.00011,0.00036,1.9e-05,0.00043,0.00016,0.0004,0.00043,1,1 +31190000,0.78,0.0016,-0.002,-0.63,1.3,1.1,0.79,9.2,6.7,-3.7e+02,-0.0011,-0.0058,-0.00019,0.012,-0.0056,-0.12,-0.17,-0.038,0.46,-0.00088,-0.029,-0.027,0,0,0.00031,0.00039,0.0041,0.13,0.2,0.0072,2.5,3.3,0.035,3.2e-07,5e-07,3.2e-06,0.029,0.025,0.00011,0.00036,1.9e-05,0.00043,0.00016,0.0004,0.00043,1,1 +31290000,0.78,0.0012,-0.0018,-0.63,1.2,1.1,0.79,9.3,6.8,-3.7e+02,-0.0011,-0.0058,-0.00019,0.011,-0.0028,-0.12,-0.17,-0.038,0.46,-0.00081,-0.029,-0.027,0,0,0.00031,0.00039,0.0041,0.13,0.21,0.0071,2.6,3.5,0.035,3.2e-07,5e-07,3.2e-06,0.029,0.025,0.00011,0.00036,1.9e-05,0.00043,0.00016,0.0004,0.00043,1,1 +31390000,0.78,0.00059,-0.0016,-0.63,1.2,1.1,0.79,9.4,6.9,-3.7e+02,-0.0011,-0.0058,-0.00019,0.0099,0.0002,-0.12,-0.17,-0.038,0.46,-0.00074,-0.029,-0.027,0,0,0.0003,0.0004,0.0041,0.13,0.22,0.0071,2.7,3.6,0.035,3.1e-07,5e-07,3.2e-06,0.029,0.025,0.00011,0.00036,1.9e-05,0.00043,0.00016,0.0004,0.00042,1,1 +31490000,0.78,8.5e-05,-0.0015,-0.63,1.2,1.1,0.79,9.5,7,-3.7e+02,-0.0011,-0.0058,-0.0002,0.0086,0.0029,-0.12,-0.17,-0.038,0.46,-0.00061,-0.029,-0.027,0,0,0.0003,0.0004,0.004,0.14,0.23,0.0071,2.8,3.8,0.035,3.1e-07,5e-07,3.2e-06,0.029,0.025,0.00011,0.00036,1.9e-05,0.00043,0.00016,0.0004,0.00042,1,1 +31590000,0.78,-0.00021,-0.0014,-0.63,1.1,1.1,0.78,9.7,7.1,-3.7e+02,-0.0011,-0.0058,-0.0002,0.0073,0.005,-0.11,-0.17,-0.038,0.46,-0.00056,-0.029,-0.027,0,0,0.0003,0.0004,0.004,0.14,0.24,0.0071,2.9,3.9,0.035,3.1e-07,5e-07,3.2e-06,0.029,0.025,0.0001,0.00036,1.9e-05,0.00043,0.00016,0.0004,0.00042,1,1 +31690000,0.78,-0.00082,-0.0013,-0.63,1.1,1.1,0.79,9.8,7.2,-3.7e+02,-0.0011,-0.0058,-0.0002,0.006,0.0077,-0.11,-0.17,-0.038,0.46,-0.00049,-0.029,-0.027,0,0,0.0003,0.0004,0.004,0.14,0.24,0.007,3,4.1,0.035,3.1e-07,5e-07,3.2e-06,0.029,0.024,0.0001,0.00036,1.9e-05,0.00042,0.00016,0.00039,0.00042,1,1 +31790000,0.78,-0.0014,-0.0011,-0.63,1.1,1,0.79,9.9,7.3,-3.7e+02,-0.0011,-0.0058,-0.0002,0.0047,0.011,-0.11,-0.17,-0.038,0.46,-0.00042,-0.029,-0.027,0,0,0.0003,0.0004,0.004,0.14,0.25,0.007,3.1,4.3,0.035,3.1e-07,5e-07,3.2e-06,0.029,0.024,0.0001,0.00036,1.9e-05,0.00042,0.00016,0.00039,0.00042,1,1 +31890000,0.78,-0.002,-0.001,-0.63,1.1,1,0.78,10,7.4,-3.7e+02,-0.0011,-0.0058,-0.0002,0.0033,0.014,-0.11,-0.17,-0.038,0.46,-0.00033,-0.029,-0.027,0,0,0.0003,0.0004,0.004,0.15,0.26,0.007,3.2,4.5,0.035,3.1e-07,5e-07,3.2e-06,0.029,0.024,0.0001,0.00036,1.9e-05,0.00042,0.00016,0.00039,0.00042,1,1 +31990000,0.78,-0.0024,-0.0009,-0.63,1,1,0.78,10,7.6,-3.7e+02,-0.0011,-0.0058,-0.00021,0.0018,0.017,-0.11,-0.17,-0.038,0.46,-0.00022,-0.029,-0.027,0,0,0.00029,0.00041,0.004,0.15,0.27,0.007,3.3,4.7,0.035,3e-07,5e-07,3.2e-06,0.029,0.024,0.0001,0.00036,1.9e-05,0.00042,0.00016,0.00039,0.00042,1,1 +32090000,0.78,-0.003,-0.0007,-0.63,1,0.99,0.79,10,7.7,-3.7e+02,-0.0012,-0.0058,-0.00021,0.00019,0.02,-0.11,-0.17,-0.038,0.46,-0.00011,-0.029,-0.027,0,0,0.00029,0.00041,0.0039,0.15,0.28,0.007,3.5,4.9,0.035,3e-07,5e-07,3.2e-06,0.029,0.024,0.0001,0.00036,1.9e-05,0.00042,0.00016,0.00039,0.00042,1,1 +32190000,0.78,-0.0038,-0.00051,-0.63,0.97,0.98,0.78,10,7.8,-3.7e+02,-0.0012,-0.0058,-0.00022,-0.0015,0.024,-0.11,-0.18,-0.038,0.46,3.2e-05,-0.03,-0.027,0,0,0.00029,0.00041,0.0039,0.15,0.29,0.0069,3.6,5.1,0.035,3e-07,5e-07,3.2e-06,0.029,0.023,9.9e-05,0.00036,1.9e-05,0.00042,0.00016,0.00039,0.00042,1,1 +32290000,0.78,-0.0044,-0.00042,-0.63,0.94,0.96,0.78,11,7.9,-3.7e+02,-0.0012,-0.0058,-0.00022,-0.0032,0.028,-0.11,-0.18,-0.038,0.46,0.00014,-0.03,-0.026,0,0,0.00029,0.00041,0.0039,0.16,0.3,0.0069,3.7,5.4,0.035,3e-07,5e-07,3.2e-06,0.029,0.023,9.8e-05,0.00036,1.9e-05,0.00042,0.00016,0.00039,0.00042,1,1 +32390000,0.78,-0.0049,-0.00032,-0.63,0.91,0.94,0.78,11,8,-3.7e+02,-0.0012,-0.0058,-0.00022,-0.0041,0.03,-0.11,-0.18,-0.038,0.46,0.00019,-0.03,-0.026,0,0,0.00029,0.00041,0.0039,0.16,0.31,0.0069,3.8,5.6,0.035,3e-07,5e-07,3.2e-06,0.029,0.023,9.7e-05,0.00036,1.9e-05,0.00042,0.00016,0.00039,0.00042,1,1 +32490000,0.78,-0.0051,-0.00027,-0.63,0.88,0.92,0.78,11,8.1,-3.7e+02,-0.0012,-0.0058,-0.00022,-0.0056,0.032,-0.11,-0.18,-0.038,0.46,0.00028,-0.03,-0.026,0,0,0.00028,0.00042,0.0039,0.16,0.32,0.0068,4,5.8,0.035,3e-07,5e-07,3.2e-06,0.029,0.023,9.6e-05,0.00036,1.9e-05,0.00042,0.00016,0.00039,0.00041,1,1 +32590000,0.78,-0.0053,-0.00021,-0.63,-1.6,-0.84,0.63,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0058,-0.00023,-0.0064,0.033,-0.11,-0.18,-0.038,0.46,0.00034,-0.03,-0.026,0,0,0.00028,0.00042,0.0039,0.25,0.25,0.56,0.25,0.25,0.037,3e-07,5.1e-07,3.2e-06,0.029,0.023,9.6e-05,0.00036,1.9e-05,0.00042,0.00016,0.00039,0.00041,1,1 +32690000,0.78,-0.0053,-0.00025,-0.63,-1.6,-0.85,0.61,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0058,-0.00023,-0.0073,0.035,-0.11,-0.18,-0.038,0.46,0.00041,-0.03,-0.026,0,0,0.00028,0.00042,0.0039,0.25,0.25,0.55,0.26,0.26,0.048,2.9e-07,5.1e-07,3.2e-06,0.029,0.023,9.5e-05,0.00036,1.9e-05,0.00041,0.00016,0.00039,0.00041,1,1 +32790000,0.78,-0.0052,-0.00026,-0.63,-1.5,-0.83,0.62,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0058,-0.00023,-0.0081,0.036,-0.11,-0.18,-0.038,0.46,0.00047,-0.03,-0.026,0,0,0.00028,0.00042,0.0039,0.13,0.13,0.27,0.26,0.26,0.049,2.9e-07,5.1e-07,3.2e-06,0.029,0.022,9.4e-05,0.00036,1.9e-05,0.00041,0.00016,0.00039,0.00041,1,1 +32890000,0.78,-0.005,-0.00039,-0.63,-1.6,-0.85,0.59,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0058,-0.00024,-0.0089,0.038,-0.11,-0.18,-0.038,0.46,0.00057,-0.03,-0.026,0,0,0.00028,0.00042,0.0039,0.13,0.13,0.26,0.27,0.27,0.059,2.9e-07,5.1e-07,3.2e-06,0.029,0.022,9.4e-05,0.00036,1.9e-05,0.00041,0.00016,0.00039,0.00041,1,1 +32990000,0.78,-0.0049,-0.0005,-0.63,-1.5,-0.84,0.59,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0058,-0.00023,-0.0098,0.04,-0.11,-0.18,-0.038,0.46,0.00059,-0.03,-0.025,0,0,0.00028,0.00042,0.0038,0.084,0.085,0.17,0.27,0.27,0.057,2.9e-07,5.1e-07,3.2e-06,0.029,0.022,9.4e-05,0.00036,1.9e-05,0.00041,0.00016,0.00039,0.00041,1,1 +33090000,0.78,-0.005,-0.00048,-0.63,-1.6,-0.86,0.58,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0058,-0.00023,-0.01,0.04,-0.11,-0.18,-0.038,0.46,0.00061,-0.03,-0.025,0,0,0.00028,0.00042,0.0038,0.084,0.086,0.16,0.28,0.28,0.065,2.9e-07,5.1e-07,3.2e-06,0.029,0.022,9.4e-05,0.00036,1.9e-05,0.00041,0.00016,0.00039,0.00041,1,1 +33190000,0.78,-0.0036,-0.0037,-0.62,-1.5,-0.84,0.53,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0058,-0.00022,-0.01,0.041,-0.11,-0.18,-0.038,0.46,0.0006,-0.03,-0.025,0,0,0.00028,0.00042,0.0038,0.063,0.065,0.11,0.28,0.28,0.062,2.9e-07,5.1e-07,3.2e-06,0.029,0.022,9.3e-05,0.00036,1.9e-05,0.00041,0.00016,0.00039,0.00041,1,1 +33290000,0.82,-0.0015,-0.016,-0.57,-1.5,-0.86,0.5,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0058,-0.00023,-0.01,0.04,-0.11,-0.18,-0.039,0.46,0.00058,-0.03,-0.025,0,0,0.00027,0.00042,0.0038,0.064,0.066,0.11,0.29,0.29,0.067,2.9e-07,5.1e-07,3.2e-06,0.029,0.022,9.3e-05,0.00035,1.9e-05,0.00041,0.00016,0.00039,0.00041,1,1 +33390000,0.89,-0.0018,-0.013,-0.46,-1.5,-0.85,0.7,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0058,-0.00022,-0.015,0.038,-0.11,-0.18,-0.039,0.46,0.0012,-0.028,-0.025,0,0,0.00028,0.0004,0.0037,0.051,0.053,0.083,0.29,0.29,0.065,2.9e-07,5e-07,3.2e-06,0.028,0.022,9.3e-05,0.00033,1.7e-05,0.00041,0.00015,0.00035,0.00041,1,1 +33490000,0.95,-0.00026,-0.0052,-0.31,-1.5,-0.86,0.72,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0058,-0.00023,-0.018,0.037,-0.11,-0.18,-0.04,0.46,0.0017,-0.02,-0.025,0,0,0.00031,0.00036,0.0034,0.052,0.054,0.075,0.3,0.3,0.068,2.8e-07,5e-07,3.2e-06,0.028,0.022,9.3e-05,0.00025,1.4e-05,0.00041,0.00013,0.00026,0.00041,1,1 +33590000,0.99,-0.003,0.0015,-0.14,-1.5,-0.84,0.68,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0058,-0.0002,-0.018,0.037,-0.11,-0.19,-0.042,0.46,0.0025,-0.013,-0.026,0,0,0.00035,0.00031,0.003,0.044,0.047,0.061,0.3,0.3,0.065,2.8e-07,5e-07,3.1e-06,0.028,0.022,9.3e-05,0.00017,1e-05,0.00041,9.5e-05,0.00016,0.0004,1,1 +33690000,1,-0.0064,0.005,0.024,-1.6,-0.86,0.68,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0058,-0.00022,-0.018,0.037,-0.11,-0.19,-0.043,0.46,0.0018,-0.0093,-0.026,0,0,0.00037,0.00028,0.0026,0.045,0.05,0.056,0.31,0.31,0.068,2.8e-07,5e-07,3.1e-06,0.028,0.022,9.3e-05,0.00013,7.8e-06,0.0004,6.9e-05,0.0001,0.0004,1,1 +33790000,0.98,-0.0073,0.0069,0.19,-1.6,-0.86,0.67,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.0002,-0.018,0.037,-0.11,-0.2,-0.043,0.46,0.002,-0.0068,-0.027,0,0,0.00037,0.00026,0.0023,0.04,0.045,0.047,0.31,0.31,0.064,2.8e-07,4.9e-07,3.1e-06,0.028,0.022,9.3e-05,9.7e-05,6.4e-06,0.0004,4.8e-05,6.3e-05,0.0004,1,1 +33890000,0.94,-0.0075,0.0082,0.35,-1.7,-0.9,0.66,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00019,-0.018,0.037,-0.11,-0.2,-0.043,0.46,0.0019,-0.0054,-0.027,0,0,0.00036,0.00026,0.0022,0.044,0.051,0.043,0.32,0.32,0.065,2.8e-07,4.9e-07,3e-06,0.028,0.022,9.3e-05,8.1e-05,5.6e-06,0.0004,3.4e-05,4.2e-05,0.0004,1,1 +33990000,0.87,-0.0095,0.0057,0.49,-1.7,-0.91,0.64,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.0002,-0.015,0.036,-0.11,-0.2,-0.044,0.46,0.0017,-0.004,-0.027,0,0,0.00032,0.00027,0.002,0.041,0.049,0.036,0.32,0.32,0.062,2.8e-07,4.8e-07,3e-06,0.028,0.022,9.3e-05,7.1e-05,5.1e-06,0.0004,2.6e-05,3e-05,0.0004,1,1 +34090000,0.81,-0.011,0.0044,0.59,-1.7,-0.97,0.65,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.0002,-0.01,0.035,-0.11,-0.2,-0.044,0.46,0.0011,-0.0034,-0.027,0,0,0.0003,0.00028,0.002,0.047,0.057,0.034,0.33,0.33,0.063,2.8e-07,4.9e-07,3e-06,0.027,0.022,9.3e-05,6.6e-05,4.9e-06,0.0004,2.1e-05,2.4e-05,0.0004,1,1 +34190000,0.76,-0.0081,0.0029,0.65,-1.7,-0.97,0.66,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.0002,-0.039,0.052,-0.11,-0.2,-0.044,0.46,0.0012,-0.0028,-0.027,0,0,0.00026,0.00028,0.0018,0.045,0.054,0.029,0.33,0.33,0.06,2.8e-07,4.7e-07,3e-06,0.026,0.021,9.3e-05,6e-05,4.6e-06,0.0004,1.7e-05,1.9e-05,0.0004,1,1 +34290000,0.72,-0.0052,0.0041,0.69,-1.7,-1,0.66,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.0002,-0.038,0.051,-0.11,-0.2,-0.044,0.46,0.0011,-0.0024,-0.027,0,0,0.00025,0.00029,0.0018,0.053,0.064,0.027,0.34,0.34,0.06,2.8e-07,4.7e-07,3e-06,0.025,0.021,9.3e-05,5.7e-05,4.5e-06,0.0004,1.4e-05,1.6e-05,0.0004,1,1 +34390000,0.7,-0.0024,0.0054,0.71,-1.8,-1.1,0.66,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.0002,-0.036,0.05,-0.11,-0.2,-0.044,0.46,0.00092,-0.0022,-0.027,0,0,0.00025,0.00029,0.0018,0.062,0.075,0.025,0.35,0.35,0.06,2.8e-07,4.8e-07,3e-06,0.025,0.02,9.3e-05,5.5e-05,4.4e-06,0.0004,1.3e-05,1.4e-05,0.0004,1,1 +34490000,0.69,-0.00036,0.0066,0.73,-1.8,-1.1,0.66,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.0002,-0.035,0.049,-0.11,-0.2,-0.044,0.46,0.0008,-0.0021,-0.027,0,0,0.00024,0.0003,0.0017,0.073,0.088,0.023,0.36,0.36,0.06,2.8e-07,4.8e-07,3e-06,0.025,0.02,9.3e-05,5.4e-05,4.3e-06,0.0004,1.1e-05,1.2e-05,0.0004,1,1 +34590000,0.68,0.00092,0.0074,0.74,-1.9,-1.2,0.67,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.0002,-0.034,0.05,-0.11,-0.2,-0.044,0.46,0.00064,-0.002,-0.027,0,0,0.00024,0.0003,0.0017,0.085,0.1,0.021,0.38,0.38,0.059,2.8e-07,4.8e-07,3e-06,0.025,0.02,9.3e-05,5.2e-05,4.3e-06,0.0004,1e-05,1.1e-05,0.0004,1,1 +34690000,0.67,0.0017,0.0079,0.74,-1.9,-1.2,0.67,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00019,-0.034,0.049,-0.11,-0.2,-0.044,0.46,0.00068,-0.0018,-0.027,0,0,0.00024,0.0003,0.0017,0.098,0.12,0.019,0.39,0.4,0.059,2.8e-07,4.8e-07,3e-06,0.025,0.02,9.3e-05,5.1e-05,4.2e-06,0.0004,9.6e-06,1e-05,0.0004,1,1 +34790000,0.67,0.0024,0.0081,0.75,-2,-1.3,0.67,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00019,-0.036,0.047,-0.11,-0.2,-0.044,0.46,0.00078,-0.0017,-0.027,0,0,0.00024,0.0003,0.0017,0.11,0.14,0.018,0.41,0.42,0.058,2.8e-07,4.8e-07,3e-06,0.025,0.02,9.3e-05,5.1e-05,4.2e-06,0.0004,8.9e-06,9.2e-06,0.0004,1,1 +34890000,0.66,0.0025,0.0082,0.75,-2,-1.3,0.67,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00019,-0.035,0.047,-0.11,-0.2,-0.044,0.46,0.00069,-0.0017,-0.027,0,0,0.00023,0.0003,0.0017,0.13,0.16,0.017,0.43,0.44,0.056,2.9e-07,4.8e-07,3e-06,0.025,0.02,9.3e-05,5e-05,4.1e-06,0.0004,8.4e-06,8.5e-06,0.0004,1,1 +34990000,0.66,-0.00086,0.016,0.75,-3,-2.2,-0.15,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00019,-0.035,0.047,-0.11,-0.2,-0.044,0.46,0.00079,-0.0017,-0.027,0,0,0.00024,0.00031,0.0017,0.16,0.22,0.016,0.46,0.47,0.056,2.9e-07,4.8e-07,3e-06,0.025,0.02,9.3e-05,4.9e-05,4.1e-06,0.0004,7.9e-06,8e-06,0.0004,1,1 +35090000,0.66,-0.00092,0.016,0.75,-3.1,-2.3,-0.2,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00019,-0.035,0.047,-0.11,-0.2,-0.044,0.46,0.00078,-0.0017,-0.027,0,0,0.00023,0.00031,0.0017,0.18,0.25,0.015,0.49,0.51,0.055,2.9e-07,4.8e-07,3e-06,0.025,0.02,9.3e-05,4.9e-05,4.1e-06,0.0004,7.5e-06,7.5e-06,0.0004,1,1 +35190000,0.66,-0.001,0.015,0.75,-3.1,-2.3,-0.19,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00019,-0.035,0.047,-0.11,-0.2,-0.044,0.46,0.00081,-0.0017,-0.027,0,0,0.00023,0.00031,0.0017,0.2,0.28,0.014,0.52,0.55,0.054,2.9e-07,4.8e-07,3e-06,0.025,0.02,9.3e-05,4.8e-05,4e-06,0.0004,7.2e-06,7.1e-06,0.0004,1,1 +35290000,0.66,-0.0012,0.015,0.75,-3.2,-2.3,-0.18,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00019,-0.035,0.047,-0.11,-0.2,-0.044,0.46,0.00084,-0.0017,-0.027,0,0,0.00023,0.00031,0.0017,0.23,0.31,0.013,0.56,0.6,0.052,2.9e-07,4.8e-07,3e-06,0.025,0.02,9.3e-05,4.8e-05,4e-06,0.0004,6.9e-06,6.7e-06,0.0004,1,1 +35390000,0.66,-0.0011,0.015,0.75,-3.2,-2.4,-0.17,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.0002,-0.035,0.047,-0.11,-0.2,-0.044,0.46,0.00091,-0.0017,-0.027,0,0,0.00023,0.00031,0.0017,0.25,0.34,0.013,0.61,0.66,0.052,2.9e-07,4.8e-07,3e-06,0.025,0.02,9.3e-05,4.7e-05,4e-06,0.0004,6.6e-06,6.4e-06,0.0004,1,1 +35490000,0.66,-0.0012,0.016,0.75,-3.2,-2.4,-0.16,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.0002,-0.035,0.047,-0.11,-0.2,-0.044,0.46,0.00097,-0.0017,-0.027,0,0,0.00023,0.0003,0.0017,0.28,0.37,0.012,0.66,0.73,0.051,2.9e-07,4.8e-07,3e-06,0.025,0.02,9.3e-05,4.7e-05,4e-06,0.0004,6.4e-06,6.2e-06,0.0004,1,1 +35590000,0.66,-0.0012,0.016,0.75,-3.3,-2.5,-0.16,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.0002,-0.035,0.047,-0.11,-0.2,-0.044,0.46,0.00092,-0.0017,-0.027,0,0,0.00023,0.0003,0.0017,0.31,0.4,0.011,0.72,0.81,0.05,2.9e-07,4.8e-07,3e-06,0.025,0.02,9.3e-05,4.6e-05,4e-06,0.0004,6.2e-06,5.9e-06,0.0004,1,1 +35690000,0.66,-0.0011,0.016,0.75,-3.3,-2.5,-0.15,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.0002,-0.035,0.046,-0.11,-0.2,-0.044,0.46,0.00098,-0.0017,-0.027,0,0,0.00023,0.0003,0.0017,0.33,0.44,0.011,0.79,0.9,0.049,2.9e-07,4.9e-07,3e-06,0.025,0.02,9.3e-05,4.6e-05,3.9e-06,0.0004,6.1e-06,5.7e-06,0.0004,1,1 +35790000,0.66,-0.0012,0.016,0.75,-3.3,-2.6,-0.14,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.0002,-0.035,0.045,-0.11,-0.2,-0.044,0.46,0.00098,-0.0017,-0.027,0,0,0.00023,0.0003,0.0017,0.36,0.48,0.01,0.86,1,0.048,2.9e-07,4.9e-07,3e-06,0.025,0.02,9.3e-05,4.6e-05,3.9e-06,0.0004,5.9e-06,5.5e-06,0.0004,1,1 +35890000,0.66,-0.0013,0.016,0.75,-3.4,-2.6,-0.14,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.0002,-0.035,0.045,-0.11,-0.2,-0.044,0.46,0.00098,-0.0017,-0.027,0,0,0.00022,0.0003,0.0017,0.4,0.52,0.01,0.95,1.1,0.047,2.9e-07,4.9e-07,3e-06,0.025,0.02,9.3e-05,4.5e-05,3.9e-06,0.0004,5.7e-06,5.3e-06,0.0004,1,1 +35990000,0.66,-0.0013,0.016,0.75,-3.4,-2.7,-0.13,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.0002,-0.034,0.045,-0.11,-0.2,-0.044,0.46,0.00094,-0.0017,-0.027,0,0,0.00022,0.0003,0.0017,0.43,0.56,0.0097,1,1.2,0.047,2.9e-07,4.9e-07,3e-06,0.025,0.02,9.4e-05,4.5e-05,3.9e-06,0.0004,5.6e-06,5.1e-06,0.0004,1,1 +36090000,0.66,-0.0013,0.016,0.75,-3.4,-2.7,-0.12,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.0002,-0.034,0.044,-0.11,-0.2,-0.044,0.46,0.00096,-0.0017,-0.027,0,0,0.00022,0.0003,0.0017,0.46,0.6,0.0093,1.2,1.4,0.046,2.9e-07,4.9e-07,3e-06,0.025,0.02,9.4e-05,4.5e-05,3.9e-06,0.0004,5.5e-06,5e-06,0.0004,1,1 +36190000,0.66,-0.0013,0.016,0.75,-3.5,-2.8,-0.11,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00022,-0.034,0.043,-0.11,-0.2,-0.044,0.46,0.001,-0.0017,-0.027,0,0,0.00022,0.0003,0.0017,0.5,0.65,0.009,1.3,1.5,0.045,3e-07,4.9e-07,3e-06,0.025,0.02,9.4e-05,4.4e-05,3.8e-06,0.0004,5.4e-06,4.8e-06,0.0004,1,1 +36290000,0.66,-0.0013,0.016,0.75,-3.5,-2.8,-0.1,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00022,-0.033,0.042,-0.11,-0.2,-0.044,0.46,0.001,-0.0017,-0.027,0,0,0.00022,0.0003,0.0017,0.53,0.69,0.0088,1.4,1.7,0.045,3e-07,4.9e-07,3e-06,0.025,0.02,9.4e-05,4.4e-05,3.8e-06,0.0004,5.3e-06,4.7e-06,0.0004,1,1 +36390000,0.66,-0.0013,0.016,0.75,-3.5,-2.9,-0.097,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00022,-0.033,0.042,-0.11,-0.2,-0.044,0.46,0.001,-0.0017,-0.027,0,0,0.00022,0.00029,0.0017,0.57,0.74,0.0086,1.6,1.9,0.044,3e-07,4.9e-07,3e-06,0.025,0.02,9.4e-05,4.4e-05,3.8e-06,0.0004,5.2e-06,4.6e-06,0.0004,1,1 +36490000,0.66,-0.0014,0.016,0.75,-3.6,-2.9,-0.09,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00021,-0.032,0.041,-0.11,-0.2,-0.044,0.46,0.00099,-0.0017,-0.027,0,0,0.00022,0.00029,0.0017,0.61,0.79,0.0083,1.7,2.1,0.043,3e-07,4.9e-07,3e-06,0.025,0.02,9.4e-05,4.3e-05,3.8e-06,0.0004,5.2e-06,4.5e-06,0.0004,1,1 +36590000,0.66,-0.0014,0.016,0.75,-3.6,-3,-0.08,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00022,-0.032,0.04,-0.11,-0.2,-0.044,0.46,0.001,-0.0016,-0.027,0,0,0.00022,0.00029,0.0017,0.65,0.84,0.0082,1.9,2.4,0.042,3e-07,4.9e-07,3e-06,0.025,0.02,9.4e-05,4.3e-05,3.8e-06,0.00039,5.1e-06,4.4e-06,0.0004,1,1 +36690000,0.66,-0.0014,0.016,0.75,-3.6,-3,-0.073,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00023,-0.031,0.04,-0.11,-0.2,-0.044,0.46,0.001,-0.0016,-0.027,0,0,0.00022,0.00029,0.0017,0.69,0.89,0.0081,2.1,2.6,0.042,3e-07,4.9e-07,3e-06,0.025,0.02,9.4e-05,4.3e-05,3.8e-06,0.00039,5e-06,4.3e-06,0.0004,1,1 +36790000,0.66,-0.0014,0.016,0.75,-3.7,-3.1,-0.063,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00024,-0.031,0.038,-0.11,-0.2,-0.044,0.46,0.0011,-0.0017,-0.027,0,0,0.00021,0.00029,0.0017,0.74,0.94,0.0079,2.3,2.9,0.041,3e-07,4.9e-07,3e-06,0.025,0.019,9.4e-05,4.3e-05,3.8e-06,0.00039,5e-06,4.2e-06,0.0004,1,1 +36890000,0.66,-0.0014,0.016,0.75,-3.7,-3.1,-0.056,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00025,-0.031,0.038,-0.11,-0.2,-0.044,0.46,0.0011,-0.0017,-0.027,0,0,0.00021,0.00029,0.0017,0.78,1,0.0078,2.6,3.2,0.041,3e-07,4.9e-07,3e-06,0.025,0.019,9.4e-05,4.2e-05,3.7e-06,0.00039,4.9e-06,4.1e-06,0.0004,1,1 +36990000,0.66,-0.0014,0.016,0.75,-3.7,-3.2,-0.049,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00025,-0.03,0.038,-0.11,-0.2,-0.044,0.46,0.0011,-0.0016,-0.027,0,0,0.00021,0.00029,0.0017,0.82,1.1,0.0077,2.8,3.6,0.04,3e-07,4.9e-07,3e-06,0.025,0.019,9.4e-05,4.2e-05,3.7e-06,0.00039,4.9e-06,4e-06,0.0004,1,1 +37090000,0.66,-0.0014,0.016,0.75,-3.8,-3.2,-0.041,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00025,-0.03,0.037,-0.11,-0.2,-0.044,0.46,0.0011,-0.0016,-0.027,0,0,0.00021,0.00029,0.0017,0.87,1.1,0.0076,3.1,4,0.04,3e-07,4.9e-07,3e-06,0.024,0.019,9.4e-05,4.2e-05,3.7e-06,0.00039,4.8e-06,3.9e-06,0.0004,1,1 +37190000,0.66,-0.0014,0.016,0.75,-3.8,-3.3,-0.033,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00025,-0.029,0.037,-0.11,-0.2,-0.044,0.46,0.0011,-0.0016,-0.027,0,0,0.00021,0.00029,0.0017,0.92,1.2,0.0076,3.4,4.4,0.039,3e-07,4.9e-07,3e-06,0.024,0.019,9.4e-05,4.2e-05,3.7e-06,0.00039,4.8e-06,3.9e-06,0.0004,1,1 +37290000,0.66,-0.0015,0.017,0.75,-3.8,-3.3,-0.026,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00025,-0.029,0.037,-0.11,-0.2,-0.044,0.46,0.0011,-0.0016,-0.027,0,0,0.00021,0.00028,0.0017,0.97,1.2,0.0075,3.7,4.8,0.039,3.1e-07,4.9e-07,2.9e-06,0.024,0.019,9.4e-05,4.1e-05,3.7e-06,0.00039,4.7e-06,3.8e-06,0.0004,1,1 +37390000,0.66,-0.0014,0.017,0.75,-3.9,-3.4,-0.019,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00026,-0.029,0.036,-0.11,-0.2,-0.044,0.46,0.0011,-0.0016,-0.027,0,0,0.00021,0.00028,0.0017,1,1.3,0.0075,4.1,5.3,0.039,3.1e-07,4.9e-07,2.9e-06,0.024,0.019,9.5e-05,4.1e-05,3.7e-06,0.00039,4.7e-06,3.7e-06,0.0004,1,1 +37490000,0.66,-0.0014,0.017,0.75,-3.9,-3.4,-0.011,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00027,-0.029,0.035,-0.11,-0.2,-0.044,0.46,0.0011,-0.0015,-0.027,0,0,0.00021,0.00028,0.0017,1.1,1.4,0.0074,4.4,5.8,0.038,3.1e-07,4.9e-07,2.9e-06,0.024,0.019,9.5e-05,4.1e-05,3.7e-06,0.00039,4.7e-06,3.7e-06,0.0004,1,1 +37590000,0.66,-0.0014,0.017,0.75,-3.9,-3.5,-0.0032,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00028,-0.028,0.034,-0.11,-0.2,-0.044,0.46,0.0012,-0.0015,-0.027,0,0,0.00021,0.00028,0.0017,1.1,1.4,0.0074,4.9,6.3,0.038,3.1e-07,4.9e-07,2.9e-06,0.024,0.019,9.5e-05,4.1e-05,3.7e-06,0.00039,4.6e-06,3.6e-06,0.0004,1,1 +37690000,0.66,-0.0015,0.017,0.75,-4,-3.5,0.0061,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00029,-0.027,0.033,-0.11,-0.2,-0.044,0.46,0.0012,-0.0015,-0.027,0,0,0.00021,0.00028,0.0017,1.2,1.5,0.0074,5.3,6.9,0.038,3.1e-07,4.9e-07,2.9e-06,0.024,0.019,9.5e-05,4.1e-05,3.6e-06,0.00039,4.6e-06,3.6e-06,0.0004,1,1 +37790000,0.66,-0.0016,0.017,0.75,-4,-3.6,0.015,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00029,-0.027,0.032,-0.11,-0.2,-0.044,0.46,0.0012,-0.0015,-0.027,0,0,0.0002,0.00028,0.0017,1.2,1.5,0.0074,5.8,7.5,0.037,3.1e-07,4.9e-07,2.9e-06,0.024,0.019,9.5e-05,4e-05,3.6e-06,0.00039,4.6e-06,3.5e-06,0.0004,1,1 +37890000,0.66,-0.0016,0.017,0.75,-4,-3.6,0.022,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00029,-0.027,0.032,-0.11,-0.2,-0.044,0.46,0.0012,-0.0015,-0.027,0,0,0.0002,0.00028,0.0017,1.3,1.6,0.0074,6.3,8.2,0.037,3.1e-07,4.8e-07,2.9e-06,0.024,0.019,9.5e-05,4e-05,3.6e-06,0.00039,4.6e-06,3.4e-06,0.0004,1,1 +37990000,0.66,-0.0016,0.017,0.75,-4.1,-3.7,0.031,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00029,-0.026,0.031,-0.11,-0.2,-0.044,0.46,0.0012,-0.0015,-0.027,0,0,0.0002,0.00028,0.0017,1.3,1.7,0.0074,6.8,8.9,0.037,3.1e-07,4.8e-07,2.9e-06,0.024,0.019,9.5e-05,4e-05,3.6e-06,0.00039,4.6e-06,3.4e-06,0.0004,1,1 +38090000,0.66,-0.0017,0.017,0.75,-4.1,-3.8,0.041,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.0003,-0.025,0.03,-0.11,-0.2,-0.044,0.46,0.0012,-0.0015,-0.027,0,0,0.0002,0.00027,0.0016,1.4,1.7,0.0074,7.4,9.6,0.037,3.1e-07,4.8e-07,2.9e-06,0.024,0.019,9.5e-05,4e-05,3.6e-06,0.00039,4.5e-06,3.4e-06,0.0004,1,1 +38190000,0.66,-0.0016,0.017,0.75,-4.2,-3.8,0.048,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.0003,-0.025,0.03,-0.11,-0.2,-0.044,0.46,0.0012,-0.0015,-0.027,0,0,0.0002,0.00027,0.0016,1.4,1.8,0.0074,8,10,0.036,3.1e-07,4.8e-07,2.9e-06,0.024,0.019,9.5e-05,3.9e-05,3.6e-06,0.00039,4.5e-06,3.3e-06,0.0004,1,1 +38290000,0.66,-0.0017,0.017,0.75,-4.2,-3.9,0.056,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.0003,-0.025,0.029,-0.11,-0.2,-0.044,0.46,0.0012,-0.0015,-0.027,0,0,0.0002,0.00027,0.0016,1.5,1.9,0.0074,8.6,11,0.037,3.1e-07,4.8e-07,2.9e-06,0.024,0.019,9.4e-05,3.9e-05,3.6e-06,0.00039,4.5e-06,3.3e-06,0.0004,1,1 +38390000,0.66,-0.0016,0.017,0.75,-4.2,-3.9,0.063,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.0003,-0.024,0.028,-0.11,-0.2,-0.044,0.46,0.0012,-0.0015,-0.027,0,0,0.0002,0.00027,0.0016,1.6,2,0.0074,9.3,12,0.036,3.1e-07,4.8e-07,2.9e-06,0.024,0.018,9.4e-05,3.9e-05,3.6e-06,0.00039,4.5e-06,3.2e-06,0.0004,1,1 +38490000,0.66,-0.0016,0.017,0.75,-4.3,-4,0.07,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.0003,-0.025,0.028,-0.11,-0.2,-0.044,0.46,0.0012,-0.0015,-0.027,0,0,0.0002,0.00027,0.0016,1.6,2,0.0074,10,13,0.036,3.1e-07,4.8e-07,2.9e-06,0.024,0.018,9.4e-05,3.9e-05,3.5e-06,0.00039,4.5e-06,3.2e-06,0.0004,1,1 +38590000,0.66,-0.0016,0.017,0.75,-4.3,-4,0.076,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.0003,-0.024,0.029,-0.11,-0.2,-0.044,0.46,0.0012,-0.0015,-0.027,0,0,0.0002,0.00027,0.0016,1.7,2.1,0.0075,11,14,0.036,3.2e-07,4.8e-07,2.8e-06,0.024,0.018,9.4e-05,3.9e-05,3.5e-06,0.00039,4.5e-06,3.2e-06,0.0004,1,1 +38690000,0.66,-0.0016,0.017,0.75,-4.3,-4.1,0.082,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0058,-0.00031,-0.025,0.029,-0.11,-0.2,-0.044,0.46,0.0012,-0.0014,-0.027,0,0,0.0002,0.00027,0.0016,1.7,2.2,0.0075,12,15,0.036,3.2e-07,4.8e-07,2.8e-06,0.024,0.018,9.4e-05,3.8e-05,3.5e-06,0.00039,4.5e-06,3.1e-06,0.0004,1,1 +38790000,0.66,-0.0016,0.017,0.75,-4.4,-4.1,0.089,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0058,-0.00031,-0.025,0.028,-0.11,-0.2,-0.044,0.46,0.0012,-0.0015,-0.027,0,0,0.0002,0.00026,0.0016,1.8,2.3,0.0075,12,16,0.036,3.2e-07,4.8e-07,2.8e-06,0.024,0.018,9.4e-05,3.8e-05,3.5e-06,0.00039,4.4e-06,3.1e-06,0.0004,1,1 +38890000,0.66,-0.0017,0.017,0.75,-4.4,-4.1,0.59,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0058,-0.00031,-0.025,0.028,-0.11,-0.2,-0.044,0.46,0.0012,-0.0015,-0.027,0,0,0.00019,0.00026,0.0016,1.9,2.3,0.0075,13,17,0.036,3.2e-07,4.8e-07,2.8e-06,0.024,0.018,9.4e-05,3.8e-05,3.5e-06,0.00039,4.4e-06,3.1e-06,0.0004,1,1 diff --git a/src/modules/ekf2/test/change_indication/iris_gps.csv b/src/modules/ekf2/test/change_indication/iris_gps.csv index 11611ada8a59..6cb6d950815c 100644 --- a/src/modules/ekf2/test/change_indication/iris_gps.csv +++ b/src/modules/ekf2/test/change_indication/iris_gps.csv @@ -1,351 +1,351 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7],state[8],state[9],state[10],state[11],state[12],state[13],state[14],state[15],state[16],state[17],state[18],state[19],state[20],state[21],state[22],state[23],variance[0],variance[1],variance[2],variance[3],variance[4],variance[5],variance[6],variance[7],variance[8],variance[9],variance[10],variance[11],variance[12],variance[13],variance[14],variance[15],variance[16],variance[17],variance[18],variance[19],variance[20],variance[21],variance[22],variance[23] -10000,1,-0.011,-0.01,0.00023,0.00033,-0.00013,-0.01,1e-05,-3.9e-06,-0.00042,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0.01,0.00018,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0,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,0.01,0.01,0.00046,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0,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,0.011,0.011,0.00094,25,25,10,1e+02,1e+02,1,0.01,0.01,0.01,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -290000,1,-0.012,-0.011,0.00044,-0.0033,-0.0044,-0.046,-0.00024,-0.00025,-0.018,3.8e-09,-5.9e-09,-4.7e-10,0,0,-2.9e-06,0,0,0,0,0,0,0,0,0.011,0.012,0.00077,25,25,9.6,0.37,0.37,0.41,0.01,0.01,0.0052,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -390000,1,-0.012,-0.011,0.00049,-0.0025,-0.0059,-0.063,-0.00056,-0.00071,-0.013,-7.1e-09,-5.8e-09,-1.3e-09,0,0,8.8e-06,0,0,0,0,0,0,0,0,0.012,0.012,0.0012,25,25,8.1,0.97,0.97,0.32,0.01,0.01,0.0052,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -490000,1,-0.012,-0.012,0.00055,-0.0007,-0.0062,-0.069,-0.00015,-0.00046,-0.011,-1.2e-06,7.5e-07,-1.7e-07,0,0,1.6e-05,0,0,0,0,0,0,0,0,0.013,0.013,0.00073,7.8,7.8,5.9,0.34,0.34,0.31,0.01,0.01,0.0024,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -590000,1,-0.012,-0.012,0.00057,-0.002,-0.009,-0.12,-0.00028,-0.0012,-0.029,-1.3e-06,7.8e-07,-1.8e-07,0,0,6.4e-05,0,0,0,0,0,0,0,0,0.015,0.015,0.001,7.9,7.9,4.2,0.67,0.67,0.32,0.01,0.01,0.0024,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -690000,1,-0.012,-0.012,0.0006,5.3e-05,-0.0088,-0.05,-8e-05,-0.00078,-0.0088,-5.6e-06,1.6e-06,-5.2e-07,0,0,-9.5e-05,0,0,0,0,0,0,0,0,0.016,0.016,0.00062,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.0006,0.0022,-0.01,-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,0.018,0.018,0.00079,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.00061,0.0031,-0.0084,-0.093,0.00015,-0.0011,-0.031,-2.1e-05,1.2e-06,-7.4e-07,0,0,-3.3e-05,0,0,0,0,0,0,0,0,0.019,0.019,0.00052,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.00058,0.006,-0.0097,-0.12,0.00062,-0.002,-0.046,-2.2e-05,1.2e-06,-7.4e-07,0,0,1.3e-05,0,0,0,0,0,0,0,0,0.021,0.021,0.00064,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.00054,0.011,-0.013,-0.13,0.00077,-0.0014,-0.062,-6e-05,-1.5e-05,-1.8e-07,0,0,4.5e-05,0,0,0,0,0,0,0,0,0.023,0.023,0.00045,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.00047,0.015,-0.018,-0.11,0.0021,-0.003,-0.047,-5.7e-05,-1.3e-05,-2.2e-07,0,0,-0.00048,0,0,0,0,0,0,0,0,0.025,0.025,0.00053,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.00041,0.019,-0.018,-0.11,0.002,-0.0024,-0.048,-0.00016,-9.5e-05,3e-06,0,0,-0.00075,0,0,0,0,0,0,0,0,0.025,0.026,0.0004,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.00038,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,0.028,0.028,0.00046,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.00037,0.024,-0.02,-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,0.027,0.027,0.00036,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.00039,0.031,-0.024,-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,0.029,0.029,0.00041,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.00042,0.028,-0.02,-0.13,0.0044,-0.0037,-0.068,-0.00073,-0.00073,2.4e-05,0,0,-0.0018,0,0,0,0,0,0,0,0,0.025,0.025,0.00033,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.00038,0.036,-0.024,-0.13,0.0076,-0.006,-0.067,-0.00072,-0.00072,2.4e-05,0,0,-0.0028,0,0,0,0,0,0,0,0,0.028,0.028,0.00037,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.00037,0.043,-0.025,-0.14,0.012,-0.0084,-0.075,-0.00072,-0.00072,2.4e-05,0,0,-0.0032,0,0,0,0,0,0,0,0,0.03,0.03,0.00041,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.00036,0.035,-0.018,-0.14,0.0082,-0.0054,-0.074,-0.0011,-0.0013,3.9e-05,0,0,-0.0046,0,0,0,0,0,0,0,0,0.024,0.024,0.00033,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.00039,0.042,-0.02,-0.14,0.012,-0.0074,-0.071,-0.0011,-0.0012,3.8e-05,0,0,-0.0065,0,0,0,0,0,0,0,0,0.026,0.026,0.00037,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.00032,0.033,-0.014,-0.14,0.0081,-0.0044,-0.077,-0.0014,-0.0018,5.1e-05,0,0,-0.0075,0,0,0,0,0,0,0,0,0.02,0.02,0.0003,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.00031,0.038,-0.014,-0.14,0.012,-0.0058,-0.075,-0.0014,-0.0018,5.1e-05,0,0,-0.0098,0,0,0,0,0,0,0,0,0.022,0.022,0.00033,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.00031,0.029,-0.0099,-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,0.016,0.016,0.00027,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.00039,0.033,-0.0088,-0.14,0.011,-0.0043,-0.079,-0.0017,-0.0023,5.9e-05,0,0,-0.013,0,0,0,0,0,0,0,0,0.017,0.017,0.0003,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.00028,0.023,-0.0059,-0.15,0.0066,-0.0024,-0.085,-0.0018,-0.0027,6.4e-05,0,0,-0.015,0,0,0,0,0,0,0,0,0.013,0.013,0.00025,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.00032,0.027,-0.0051,-0.15,0.0091,-0.003,-0.084,-0.0018,-0.0027,6.4e-05,0,0,-0.018,0,0,0,0,0,0,0,0,0.014,0.014,0.00027,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.00025,0.022,-0.0028,-0.14,0.0059,-0.0016,-0.081,-0.0019,-0.003,6.7e-05,0,0,-0.022,0,0,0,0,0,0,0,0,0.011,0.011,0.00023,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.00017,0.026,-0.0046,-0.14,0.0083,-0.002,-0.081,-0.0019,-0.003,6.7e-05,0,0,-0.025,0,0,0,0,0,0,0,0,0.012,0.012,0.00025,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.00017,0.02,-0.0035,-0.15,0.0054,-0.0012,-0.086,-0.002,-0.0033,6.8e-05,0,0,-0.028,0,0,0,0,0,0,0,0,0.009,0.009,0.00022,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.00037,0.025,-0.0064,-0.15,0.0077,-0.0018,-0.087,-0.002,-0.0033,6.8e-05,0,0,-0.031,0,0,0,0,0,0,0,0,0.01,0.01,0.00023,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.00041,0.021,-0.0062,-0.15,0.0051,-0.0013,-0.097,-0.002,-0.0036,6.8e-05,0,0,-0.033,0,0,0,0,0,0,0,0,0.0078,0.0078,0.0002,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.00043,0.024,-0.0063,-0.15,0.0074,-0.002,-0.11,-0.002,-0.0036,6.8e-05,0,0,-0.034,0,0,0,0,0,0,0,0,0.0086,0.0086,0.00022,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.00043,0.019,-0.0034,-0.15,0.005,-0.0013,-0.1,-0.0021,-0.0038,6.8e-05,0,0,-0.04,0,0,0,0,0,0,0,0,0.0069,0.0069,0.00019,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.00041,0.025,-0.0021,-0.15,0.0073,-0.0016,-0.1,-0.0021,-0.0038,6.8e-05,0,0,-0.044,0,0,0,0,0,0,0,0,0.0076,0.0076,0.0002,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.0095,-0.013,0.00035,0.022,-0.0016,-0.15,0.0051,-0.00096,-0.11,-0.0021,-0.004,6.7e-05,0,0,-0.047,0,0,0,0,0,0,0,0,0.0061,0.0061,0.00018,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.00033,0.024,-0.001,-0.15,0.0075,-0.0012,-0.11,-0.0021,-0.004,6.7e-05,0,0,-0.052,0,0,0,0,0,0,0,0,0.0067,0.0068,0.00019,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.00035,0.02,0.0033,-0.15,0.0052,-0.00053,-0.11,-0.0022,-0.0042,6.5e-05,0,0,-0.055,0,0,0,0,0,0,0,0,0.0055,0.0055,0.00017,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.00042,0.022,0.0045,-0.14,0.0074,-0.00015,-0.11,-0.0022,-0.0042,6.5e-05,0,0,-0.059,0,0,0,0,0,0,0,0,0.006,0.006,0.00018,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.00048,0.027,0.0041,-0.14,0.0099,0.00021,-0.11,-0.0022,-0.0042,6.5e-05,0,0,-0.064,0,0,0,0,0,0,0,0,0.0066,0.0066,0.00019,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.00054,0.023,0.0034,-0.12,0.0074,0.00042,-0.098,-0.0021,-0.0044,6.3e-05,0,0,-0.072,0,0,0,0,0,0,0,0,0.0053,0.0053,0.00017,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.0094,-0.012,0.0005,0.025,0.003,-0.12,0.0098,0.00073,-0.1,-0.0021,-0.0044,6.3e-05,0,0,-0.074,0,0,0,0,0,0,0,0,0.0058,0.0058,0.00018,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.0005,0.023,0.0029,-0.12,0.0071,0.00062,-0.11,-0.0021,-0.0046,6.1e-05,0,0,-0.077,0,0,0,0,0,0,0,0,0.0047,0.0047,0.00016,0.46,0.46,0.084,0.15,0.15,0.085,0.00095,0.00095,1.6e-05,0.04,0.04,0.017,0,0,0,0,0,0,0,0 -4390000,1,-0.0094,-0.012,0.00045,0.026,0.0013,-0.11,0.0097,0.00073,-0.094,-0.0021,-0.0046,6.1e-05,0,0,-0.083,0,0,0,0,0,0,0,0,0.0052,0.0052,0.00017,0.55,0.55,0.081,0.2,0.2,0.084,0.00095,0.00095,1.6e-05,0.04,0.04,0.016,0,0,0,0,0,0,0,0 -4490000,1,-0.0095,-0.012,0.00051,0.022,0.003,-0.11,0.0071,0.00061,-0.095,-0.0021,-0.0048,5.8e-05,0,0,-0.086,0,0,0,0,0,0,0,0,0.0042,0.0042,0.00015,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.00057,0.025,0.0018,-0.11,0.0095,0.00084,-0.098,-0.0021,-0.0048,5.8e-05,0,0,-0.089,0,0,0,0,0,0,0,0,0.0045,0.0045,0.00016,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.00049,0.019,0.0021,-0.1,0.0069,0.0006,-0.09,-0.0021,-0.005,5.6e-05,0,0,-0.093,0,0,0,0,0,0,0,0,0.0037,0.0037,0.00015,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.0093,-0.012,0.00059,0.017,0.0041,-0.099,0.0087,0.00095,-0.092,-0.0021,-0.005,5.6e-05,0,0,-0.095,0,0,0,0,0,0,0,0,0.004,0.004,0.00015,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.00062,0.012,0.0017,-0.093,0.0059,0.00073,-0.088,-0.0021,-0.0051,5.4e-05,0,0,-0.099,0,0,0,0,0,0,0,0,0.0032,0.0032,0.00014,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.0006,0.016,0.0023,-0.085,0.0073,0.00095,-0.083,-0.0021,-0.0051,5.4e-05,0,0,-0.1,0,0,0,0,0,0,0,0,0.0035,0.0035,0.00015,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.0091,-0.012,0.00066,0.012,0.0027,-0.082,0.0051,0.00068,-0.081,-0.0021,-0.0052,5.2e-05,0,0,-0.1,0,0,0,0,0,0,0,0,0.0028,0.0028,0.00013,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.0007,0.012,0.0062,-0.08,0.0063,0.0011,-0.079,-0.0021,-0.0052,5.2e-05,0,0,-0.11,0,0,0,0,0,0,0,0,0.003,0.003,0.00014,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.00075,0.01,0.0064,-0.068,0.0044,0.001,-0.072,-0.002,-0.0053,5e-05,0,0,-0.11,0,0,0,0,0,0,0,0,0.0024,0.0024,0.00013,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.00075,0.0099,0.0099,-0.065,0.0054,0.0018,-0.067,-0.002,-0.0053,5e-05,0,0,-0.11,0,0,0,0,0,0,0,0,0.0026,0.0026,0.00014,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.00074,0.0091,0.011,-0.06,0.0037,0.0017,-0.065,-0.002,-0.0054,4.8e-05,0,0,-0.11,0,0,0,0,0,0,0,0,0.0021,0.0021,0.00012,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.00066,0.011,0.015,-0.053,0.0048,0.003,-0.058,-0.002,-0.0054,4.8e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.0022,0.0022,0.00013,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.00056,0.0095,0.015,-0.052,0.0034,0.0027,-0.055,-0.0019,-0.0055,4.7e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.0018,0.0018,0.00012,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.0005,0.011,0.017,-0.049,0.0045,0.0043,-0.053,-0.0019,-0.0055,4.7e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.0019,0.0019,0.00013,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.00053,0.011,0.015,-0.048,0.0034,0.0035,-0.056,-0.0019,-0.0055,4.5e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.0015,0.0015,0.00012,0.23,0.23,0.048,0.1,0.1,0.075,0.00018,0.00018,6.9e-06,0.04,0.04,0.0054,0,0,0,0,0,0,0,0 -5990000,1,-0.0088,-0.012,0.0005,0.013,0.016,-0.041,0.0046,0.005,-0.05,-0.0019,-0.0055,4.5e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.0017,0.0017,0.00012,0.27,0.27,0.045,0.13,0.13,0.074,0.00018,0.00018,6.9e-06,0.04,0.04,0.005,0,0,0,0,0,0,0,0 -6090000,1,-0.0088,-0.012,0.00031,0.013,0.017,-0.039,0.0059,0.0067,-0.047,-0.0019,-0.0055,4.5e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.0018,0.0018,0.00013,0.31,0.31,0.044,0.17,0.17,0.074,0.00018,0.00018,6.9e-06,0.04,0.04,0.0047,0,0,0,0,0,0,0,0 -6190000,-0.29,0.013,-0.0053,0.96,0.012,0.013,-0.037,0.0046,0.0053,-0.047,-0.0018,-0.0056,4.3e-05,0,0,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.0014,0.0014,0.018,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.29,0.013,-0.0052,0.96,0.014,0.011,-0.041,0.0058,0.0066,-0.053,-0.0018,-0.0056,4.3e-05,0,0,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.0014,0.0014,0.018,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.29,0.013,-0.0052,0.96,0.012,0.0091,-0.042,0.0071,0.0076,-0.056,-0.0018,-0.0056,4.3e-05,0,0,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.0014,0.0014,0.018,0.22,0.22,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.29,0.013,-0.0051,0.96,0.014,0.01,-0.039,0.0085,0.0086,-0.053,-0.0018,-0.0056,4.3e-05,0,0,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.0014,0.0014,0.018,0.23,0.23,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.29,0.013,-0.0051,0.96,0.015,0.0077,-0.042,0.0098,0.0095,-0.056,-0.0018,-0.0056,4.3e-05,0,0,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.0014,0.0014,0.018,0.24,0.24,0.036,0.27,0.27,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.29,0.013,-0.005,0.96,0.018,0.0053,-0.044,0.012,0.01,-0.057,-0.0018,-0.0056,4.3e-05,0,0,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.0014,0.0014,0.018,0.25,0.25,0.035,0.32,0.32,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.29,0.013,-0.005,0.96,0.017,0.0028,-0.042,0.013,0.011,-0.058,-0.0018,-0.0056,4.3e-05,0,0,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.0014,0.0014,0.018,0.26,0.26,0.034,0.37,0.37,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.29,0.013,-0.0048,0.96,0.017,0.0023,-0.038,0.015,0.011,-0.055,-0.0018,-0.0056,4.3e-05,0,0,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.0015,0.0015,0.018,0.27,0.27,0.032,0.43,0.43,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.29,0.013,-0.0047,0.96,0.018,0.00049,-0.037,0.017,0.011,-0.055,-0.0018,-0.0056,4.3e-05,0,0,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.0015,0.0015,0.018,0.29,0.29,0.031,0.49,0.49,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.29,0.013,-0.0046,0.96,0.02,-0.0052,-0.037,0.019,0.011,-0.056,-0.0018,-0.0056,4.3e-05,0,0,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.0015,0.0015,0.018,0.3,0.3,0.03,0.56,0.56,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.29,0.013,-0.0045,0.96,0.02,-0.0078,-0.036,0.021,0.0099,-0.058,-0.0018,-0.0056,4.2e-05,0,0,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.0015,0.0015,0.018,0.32,0.32,0.029,0.63,0.63,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.29,0.013,-0.0045,0.96,0.02,-0.013,-0.034,0.023,0.009,-0.054,-0.0018,-0.0056,4.3e-05,0,0,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.0016,0.0016,0.018,0.35,0.35,0.028,0.72,0.72,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.29,0.013,-0.0044,0.96,0.023,-0.016,-0.032,0.025,0.0074,-0.052,-0.0018,-0.0056,4.2e-05,0,0,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.0016,0.0016,0.018,0.37,0.37,0.027,0.8,0.8,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.29,0.013,-0.0043,0.96,0.021,-0.02,-0.026,0.027,0.0056,-0.046,-0.0018,-0.0056,4.2e-05,0,0,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.0016,0.0016,0.018,0.4,0.4,0.026,0.89,0.89,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.29,0.013,-0.0044,0.96,0.021,-0.024,-0.023,0.029,0.0034,-0.041,-0.0018,-0.0056,4.2e-05,0,0,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.0017,0.0017,0.018,0.42,0.42,0.025,0.99,0.99,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.29,0.013,-0.0044,0.96,0.022,-0.029,-0.022,0.031,0.00066,-0.036,-0.0018,-0.0056,4.2e-05,0,0,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.0017,0.0017,0.018,0.46,0.46,0.025,1.1,1.1,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.29,0.013,-0.0043,0.96,0.021,-0.032,-0.024,0.033,-0.0023,-0.041,-0.0018,-0.0056,4.2e-05,0,0,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.0018,0.0018,0.018,0.49,0.49,0.024,1.2,1.2,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.29,0.013,-0.0042,0.96,0.023,-0.037,-0.025,0.035,-0.0057,-0.045,-0.0018,-0.0056,4.2e-05,0,0,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.0018,0.0018,0.018,0.53,0.53,0.023,1.3,1.3,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.29,0.013,-0.0042,0.96,0.023,-0.041,-0.021,0.037,-0.0095,-0.041,-0.0018,-0.0056,4.2e-05,0,0,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.0019,0.0019,0.018,0.56,0.56,0.022,1.5,1.5,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.29,0.013,-0.004,0.96,0.022,-0.046,-0.022,0.039,-0.014,-0.044,-0.0018,-0.0056,4.2e-05,0,0,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.0019,0.0019,0.018,0.61,0.61,0.022,1.6,1.6,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.29,0.013,-0.0041,0.96,0.023,-0.053,-0.018,0.042,-0.019,-0.038,-0.0018,-0.0056,4.2e-05,0,0,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.002,0.002,0.018,0.65,0.65,0.021,1.8,1.8,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.29,0.013,-0.004,0.96,0.022,-0.059,-0.016,0.043,-0.024,-0.038,-0.0018,-0.0056,4.2e-05,0,0,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.0021,0.002,0.018,0.7,0.7,0.02,1.9,1.9,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.29,0.013,-0.004,0.96,0.024,-0.062,-0.015,0.045,-0.03,-0.036,-0.0018,-0.0056,4.2e-05,0,0,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.0021,0.0021,0.018,0.75,0.75,0.02,2.1,2.1,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.29,0.013,-0.0038,0.96,0.025,-0.067,-0.017,0.047,-0.036,-0.041,-0.0018,-0.0056,4.2e-05,0,0,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.0022,0.0022,0.018,0.8,0.8,0.019,2.3,2.3,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.29,0.013,-0.0038,0.96,0.025,-0.072,-0.012,0.049,-0.043,-0.036,-0.0018,-0.0056,4.2e-05,0,0,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.0023,0.0022,0.018,0.87,0.87,0.018,2.5,2.5,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.29,0.013,-0.0038,0.96,0.025,-0.075,-0.014,0.051,-0.049,-0.037,-0.0018,-0.0056,4.2e-05,0,0,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.0023,0.0023,0.018,0.92,0.92,0.018,2.7,2.7,0.055,0.00014,0.00014,6.3e-06,0.04,0.04,0.001,0,0,0,0,0,0,0,0 -8790000,-0.29,0.013,-0.0037,0.96,0.024,-0.08,-0.013,0.053,-0.057,-0.035,-0.0018,-0.0056,4.2e-05,0,0,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.0024,0.0024,0.018,0.99,0.99,0.018,3,3,0.055,0.00014,0.00014,6.3e-06,0.04,0.04,0.00096,0,0,0,0,0,0,0,0 -8890000,-0.29,0.013,-0.0038,0.96,0.024,-0.085,-0.009,0.054,-0.064,-0.029,-0.0018,-0.0056,4.2e-05,0,0,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.0025,0.0025,0.018,1,1,0.017,3.2,3.2,0.054,0.00014,0.00014,6.3e-06,0.04,0.04,0.00091,0,0,0,0,0,0,0,0 -8990000,-0.29,0.013,-0.0037,0.96,0.026,-0.091,-0.0083,0.057,-0.072,-0.032,-0.0018,-0.0056,4.2e-05,0,0,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.0025,0.0025,0.018,1.1,1.1,0.017,3.5,3.5,0.054,0.00014,0.00014,6.3e-06,0.04,0.04,0.00088,0,0,0,0,0,0,0,0 -9090000,-0.29,0.013,-0.0037,0.96,0.026,-0.096,-0.0093,0.057,-0.079,-0.032,-0.0018,-0.0056,4.2e-05,0,0,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.0026,0.0026,0.018,1.2,1.2,0.016,3.7,3.7,0.053,0.00014,0.00014,6.3e-06,0.04,0.04,0.00084,0,0,0,0,0,0,0,0 -9190000,-0.29,0.013,-0.0036,0.96,0.024,-0.1,-0.0087,0.06,-0.089,-0.032,-0.0018,-0.0056,4.3e-05,0,0,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.0027,0.0027,0.018,1.3,1.3,0.016,4,4,0.052,0.00014,0.00014,6.3e-06,0.04,0.04,0.0008,0,0,0,0,0,0,0,0 -9290000,-0.29,0.013,-0.0035,0.96,0.021,-0.1,-0.0071,0.06,-0.095,-0.03,-0.0019,-0.0056,4.3e-05,0,0,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.0028,0.0028,0.018,1.3,1.3,0.015,4.2,4.2,0.052,0.00014,0.00014,6.3e-06,0.04,0.04,0.00076,0,0,0,0,0,0,0,0 -9390000,-0.29,0.013,-0.0034,0.96,0.022,-0.11,-0.006,0.062,-0.11,-0.03,-0.0019,-0.0056,4.3e-05,0,0,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.0029,0.0029,0.018,1.4,1.4,0.015,4.6,4.6,0.052,0.00014,0.00014,6.3e-06,0.04,0.04,0.00073,0,0,0,0,0,0,0,0 -9490000,-0.29,0.013,-0.0034,0.96,0.022,-0.11,-0.0043,0.061,-0.11,-0.027,-0.0019,-0.0056,4.3e-05,0,0,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.0029,0.0029,0.018,1.5,1.5,0.015,4.8,4.8,0.051,0.00013,0.00013,6.3e-06,0.04,0.04,0.0007,0,0,0,0,0,0,0,0 -9590000,-0.29,0.013,-0.0034,0.96,0.023,-0.12,-0.0042,0.063,-0.12,-0.028,-0.0019,-0.0056,4.3e-05,0,0,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.003,0.003,0.018,1.6,1.6,0.014,5.3,5.3,0.05,0.00013,0.00013,6.3e-06,0.04,0.04,0.00067,0,0,0,0,0,0,0,0 -9690000,-0.29,0.013,-0.0034,0.96,0.023,-0.12,-0.0013,0.062,-0.13,-0.027,-0.0019,-0.0056,4.3e-05,0,0,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.0031,0.0031,0.018,1.7,1.7,0.014,5.5,5.5,0.05,0.00013,0.00013,6.3e-06,0.04,0.04,0.00065,0,0,0,0,0,0,0,0 -9790000,-0.29,0.013,-0.0034,0.96,0.022,-0.12,-0.0026,0.064,-0.14,-0.028,-0.0019,-0.0056,4.3e-05,0,0,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.0032,0.0032,0.018,1.8,1.8,0.014,6,6,0.05,0.00013,0.00013,6.3e-06,0.04,0.04,0.00062,0,0,0,0,0,0,0,0 -9890000,-0.29,0.013,-0.0034,0.96,0.019,-0.12,-0.0013,0.063,-0.14,-0.029,-0.0019,-0.0057,4.3e-05,0,0,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.0032,0.0032,0.018,1.8,1.8,0.013,6.2,6.2,0.049,0.00013,0.00013,6.3e-06,0.04,0.04,0.0006,0,0,0,0,0,0,0,0 -9990000,-0.29,0.013,-0.0033,0.96,0.018,-0.13,-0.00064,0.064,-0.16,-0.03,-0.0019,-0.0057,4.3e-05,0,0,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.0034,0.0034,0.018,2,2,0.013,6.8,6.8,0.049,0.00013,0.00013,6.3e-06,0.04,0.04,0.00058,0,0,0,0,0,0,0,0 -10090000,-0.29,0.013,-0.0033,0.96,0.019,-0.12,0.00057,0.062,-0.16,-0.029,-0.0019,-0.0057,4.2e-05,0,0,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.0034,0.0034,0.018,2,2,0.013,6.9,6.9,0.048,0.00013,0.00013,6.3e-06,0.04,0.04,0.00056,0,0,0,0,0,0,0,0 -10190000,-0.29,0.013,-0.0033,0.96,0.022,-0.13,0.0014,0.064,-0.17,-0.03,-0.0019,-0.0057,4.2e-05,0,0,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.0035,0.0035,0.018,2.1,2.1,0.012,7.5,7.5,0.048,0.00013,0.00013,6.3e-06,0.04,0.04,0.00054,0,0,0,0,0,0,0,0 -10290000,-0.29,0.013,-0.0033,0.96,0.022,-0.13,0.00039,0.066,-0.18,-0.029,-0.0019,-0.0057,4.2e-05,0,0,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.0036,0.0036,0.018,2.3,2.3,0.012,8.2,8.2,0.048,0.00013,0.00013,6.3e-06,0.04,0.04,0.00052,0,0,0,0,0,0,0,0 -10390000,-0.29,0.013,-0.0033,0.96,0.014,-0.0061,-0.0025,0.0011,-0.00019,-0.028,-0.0019,-0.0057,4.2e-05,-3.7e-08,2.2e-08,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.0038,0.0037,0.018,0.25,0.25,0.56,0.25,0.25,0.048,0.00013,0.00013,6.3e-06,0.04,0.04,0.0005,0,0,0,0,0,0,0,0 -10490000,-0.28,0.013,-0.0031,0.96,0.014,-0.011,0.007,0.0025,-0.00098,-0.023,-0.0019,-0.0057,4.3e-05,-1.3e-06,7.4e-07,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.0039,0.0039,0.018,0.26,0.26,0.55,0.26,0.26,0.057,0.00013,0.00013,6.3e-06,0.04,0.04,0.0005,0,0,0,0,0,0,0,0 -10590000,-0.28,0.013,-0.0032,0.96,0.002,-0.009,0.013,0.00046,-0.00067,-0.021,-0.0018,-0.0056,4.2e-05,-0.00021,0.00035,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.0039,0.0039,0.018,0.14,0.14,0.27,0.26,0.26,0.055,0.00013,0.00013,6.3e-06,0.04,0.04,0.0005,0,0,0,0,0,0,0,0 -10690000,-0.28,0.013,-0.0031,0.96,0.0011,-0.012,0.016,0.00061,-0.0017,-0.017,-0.0018,-0.0056,4.2e-05,-0.00021,0.00035,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.004,0.004,0.018,0.16,0.16,0.26,0.27,0.27,0.065,0.00013,0.00013,6.3e-06,0.04,0.04,0.0005,0,0,0,0,0,0,0,0 -10790000,-0.28,0.013,-0.0031,0.96,-0.00068,-0.016,0.014,0.00022,-0.0018,-0.015,-0.0018,-0.0056,4.1e-05,-0.00017,0.00043,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.0038,0.0038,0.018,0.11,0.11,0.17,0.13,0.13,0.062,0.00012,0.00012,6.3e-06,0.04,0.04,0.0005,0,0,0,0,0,0,0,0 -10890000,-0.28,0.013,-0.003,0.96,-0.00016,-0.021,0.01,0.00016,-0.0036,-0.018,-0.0018,-0.0056,4.1e-05,-0.00017,0.00043,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.0039,0.0039,0.018,0.14,0.14,0.16,0.14,0.14,0.068,0.00012,0.00012,6.3e-06,0.04,0.04,0.0005,0,0,0,0,0,0,0,0 -10990000,-0.28,0.013,-0.0034,0.96,0.0013,-0.022,0.016,0.00011,-0.0017,-0.011,-0.0017,-0.0057,3.8e-05,0.00025,0.0011,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.0035,0.0035,0.018,0.11,0.11,0.12,0.093,0.093,0.065,0.00011,0.00011,6.3e-06,0.039,0.039,0.0005,0,0,0,0,0,0,0,0 -11090000,-0.28,0.013,-0.0035,0.96,0.0012,-0.029,0.02,0.00026,-0.0042,-0.0074,-0.0017,-0.0057,3.8e-05,0.00025,0.0011,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.0036,0.0036,0.018,0.14,0.14,0.11,0.1,0.1,0.069,0.00011,0.00011,6.3e-06,0.039,0.039,0.0005,0,0,0,0,0,0,0,0 -11190000,-0.28,0.013,-0.0041,0.96,0.0068,-0.023,0.026,0.0016,-0.0019,-0.00035,-0.0016,-0.0058,3.3e-05,0.0011,0.0019,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.003,0.003,0.018,0.11,0.11,0.083,0.074,0.074,0.066,9.6e-05,9.6e-05,6.3e-06,0.038,0.038,0.0005,0,0,0,0,0,0,0,0 -11290000,-0.28,0.013,-0.004,0.96,0.0076,-0.025,0.026,0.0024,-0.0044,-0.00012,-0.0016,-0.0058,3.3e-05,0.0012,0.0019,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.0031,0.0031,0.018,0.14,0.14,0.077,0.081,0.081,0.069,9.6e-05,9.6e-05,6.3e-06,0.038,0.038,0.0005,0,0,0,0,0,0,0,0 -11390000,-0.28,0.013,-0.0045,0.96,0.0057,-0.019,0.016,0.0015,-0.002,-0.0086,-0.0014,-0.0059,2.8e-05,0.0014,0.0028,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.0025,0.0025,0.018,0.11,0.11,0.062,0.064,0.064,0.066,8.2e-05,8.2e-05,6.3e-06,0.038,0.038,0.0005,0,0,0,0,0,0,0,0 -11490000,-0.28,0.013,-0.0044,0.96,0.0072,-0.02,0.02,0.0022,-0.004,-0.0025,-0.0014,-0.0059,2.8e-05,0.0014,0.0028,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.0026,0.0026,0.018,0.14,0.14,0.057,0.072,0.072,0.067,8.2e-05,8.2e-05,6.3e-06,0.038,0.038,0.0005,0,0,0,0,0,0,0,0 -11590000,-0.28,0.013,-0.0049,0.96,0.0025,-0.015,0.018,0.0011,-0.0019,-0.0036,-0.0013,-0.0059,2.5e-05,0.0017,0.0034,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.0021,0.0021,0.018,0.11,0.11,0.048,0.058,0.058,0.065,7e-05,7e-05,6.3e-06,0.038,0.038,0.0005,0,0,0,0,0,0,0,0 -11690000,-0.28,0.013,-0.0049,0.96,0.003,-0.019,0.018,0.0014,-0.0036,-0.0049,-0.0013,-0.0059,2.5e-05,0.0017,0.0034,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.0021,0.0021,0.018,0.13,0.13,0.044,0.067,0.067,0.066,7e-05,7e-05,6.3e-06,0.038,0.038,0.0005,0,0,0,0,0,0,0,0 -11790000,-0.28,0.013,-0.0051,0.96,0.0023,-0.018,0.019,0.00096,-0.0033,-0.002,-0.0012,-0.0059,2.4e-05,0.0017,0.0037,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.0017,0.0017,0.018,0.1,0.1,0.037,0.055,0.055,0.063,6.1e-05,6.1e-05,6.3e-06,0.037,0.037,0.0005,0,0,0,0,0,0,0,0 -11890000,-0.28,0.013,-0.0052,0.96,2e-05,-0.021,0.017,0.0011,-0.0052,-0.0013,-0.0012,-0.0059,2.4e-05,0.0017,0.0037,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.0018,0.0018,0.018,0.12,0.12,0.034,0.064,0.064,0.063,6.1e-05,6.1e-05,6.3e-06,0.037,0.037,0.0005,0,0,0,0,0,0,0,0 -11990000,-0.28,0.013,-0.0057,0.96,0.0026,-0.012,0.015,0.0029,-0.0019,-0.005,-0.0011,-0.006,2e-05,0.0023,0.0041,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.0015,0.0015,0.018,0.098,0.098,0.03,0.065,0.065,0.061,5.4e-05,5.4e-05,6.3e-06,0.037,0.037,0.0005,0,0,0,0,0,0,0,0 -12090000,-0.28,0.013,-0.0056,0.96,0.0016,-0.013,0.018,0.0031,-0.0031,0.0011,-0.0011,-0.006,2e-05,0.0022,0.0041,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.0015,0.0015,0.018,0.12,0.12,0.027,0.075,0.075,0.06,5.4e-05,5.4e-05,6.3e-06,0.037,0.037,0.0005,0,0,0,0,0,0,0,0 -12190000,-0.28,0.013,-0.0058,0.96,-0.00043,-0.0041,0.017,0.0023,0.00068,0.0029,-0.0011,-0.006,1.8e-05,0.0024,0.0044,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.0013,0.0013,0.018,0.088,0.088,0.024,0.059,0.059,0.058,4.9e-05,4.9e-05,6.3e-06,0.037,0.037,0.0005,0,0,0,0,0,0,0,0 -12290000,-0.28,0.013,-0.0059,0.96,0.0019,-0.0021,0.016,0.0024,0.00035,0.0039,-0.0011,-0.006,1.8e-05,0.0024,0.0044,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.0013,0.0013,0.018,0.1,0.1,0.022,0.069,0.069,0.058,4.9e-05,4.9e-05,6.3e-06,0.037,0.037,0.0005,0,0,0,0,0,0,0,0 -12390000,-0.28,0.013,-0.0059,0.96,0.00068,-0.00091,0.014,0.0017,0.00043,-0.0021,-0.0011,-0.006,1.8e-05,0.0024,0.0043,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.0011,0.0011,0.018,0.081,0.081,0.02,0.056,0.056,0.056,4.5e-05,4.5e-05,6.3e-06,0.037,0.037,0.0005,0,0,0,0,0,0,0,0 -12490000,-0.28,0.013,-0.0059,0.96,0.00093,-0.0013,0.018,0.0017,0.00031,-7.1e-05,-0.0011,-0.006,1.8e-05,0.0024,0.0044,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.0012,0.0012,0.018,0.094,0.094,0.018,0.065,0.065,0.055,4.5e-05,4.5e-05,6.3e-06,0.037,0.037,0.0005,0,0,0,0,0,0,0,0 -12590000,-0.28,0.012,-0.0058,0.96,0.00093,-0.0026,0.02,0.003,-0.0014,0.0017,-0.0011,-0.006,1.8e-05,0.0025,0.0043,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.001,0.001,0.018,0.074,0.074,0.017,0.054,0.054,0.054,4.2e-05,4.2e-05,6.3e-06,0.037,0.037,0.0005,0,0,0,0,0,0,0,0 -12690000,-0.28,0.012,-0.0057,0.96,0.00046,-0.00051,0.019,0.0031,-0.0015,0.0033,-0.0011,-0.006,1.8e-05,0.0025,0.0043,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.0011,0.0011,0.018,0.086,0.086,0.016,0.063,0.063,0.053,4.2e-05,4.2e-05,6.3e-06,0.037,0.037,0.0005,0,0,0,0,0,0,0,0 -12790000,-0.28,0.012,-0.0056,0.96,0.0063,-0.0076,0.021,0.0066,-0.005,0.0054,-0.0012,-0.0061,2e-05,0.0025,0.0042,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.00098,0.00098,0.018,0.074,0.074,0.014,0.063,0.063,0.052,3.9e-05,3.9e-05,6.3e-06,0.037,0.037,0.0005,0,0,0,0,0,0,0,0 -12890000,-0.28,0.012,-0.0056,0.96,0.0071,-0.0074,0.022,0.0072,-0.0057,0.0085,-0.0012,-0.0061,2e-05,0.0025,0.0042,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.001,0.001,0.018,0.085,0.085,0.013,0.073,0.073,0.051,3.9e-05,3.9e-05,6.3e-06,0.037,0.037,0.0005,0,0,0,0,0,0,0,0 -12990000,-0.28,0.012,-0.0057,0.96,0.0041,-0.0056,0.022,0.005,-0.0039,0.0096,-0.0011,-0.0061,1.9e-05,0.0025,0.0042,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.00092,0.00092,0.018,0.066,0.066,0.012,0.059,0.059,0.05,3.7e-05,3.7e-05,6.3e-06,0.037,0.037,0.0005,0,0,0,0,0,0,0,0 -13090000,-0.28,0.012,-0.0057,0.96,0.0039,-0.0064,0.02,0.0054,-0.0046,0.0085,-0.0011,-0.0061,1.9e-05,0.0025,0.0042,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.00095,0.00094,0.018,0.076,0.076,0.012,0.068,0.068,0.049,3.7e-05,3.7e-05,6.3e-06,0.037,0.037,0.0005,0,0,0,0,0,0,0,0 -13190000,-0.28,0.012,-0.0057,0.96,0.0045,-0.0072,0.019,0.0053,-0.0059,0.0091,-0.0011,-0.006,1.9e-05,0.0026,0.0042,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.00089,0.00089,0.018,0.066,0.066,0.011,0.068,0.068,0.047,3.5e-05,3.5e-05,6.3e-06,0.037,0.037,0.0005,0,0,0,0,0,0,0,0 -13290000,-0.28,0.012,-0.0057,0.96,0.0051,-0.0068,0.016,0.0058,-0.0066,0.0085,-0.0011,-0.006,1.9e-05,0.0026,0.0042,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.00091,0.00091,0.018,0.075,0.075,0.01,0.079,0.079,0.047,3.5e-05,3.5e-05,6.3e-06,0.037,0.037,0.0005,0,0,0,0,0,0,0,0 -13390000,-0.28,0.012,-0.0057,0.96,0.0034,-0.0051,0.016,0.004,-0.0045,0.0091,-0.0011,-0.006,1.9e-05,0.0027,0.0041,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.00085,0.00085,0.018,0.059,0.059,0.0097,0.062,0.062,0.046,3.3e-05,3.3e-05,6.3e-06,0.037,0.037,0.0005,0,0,0,0,0,0,0,0 -13490000,-0.28,0.012,-0.0057,0.96,0.0036,-0.0073,0.015,0.0043,-0.0052,0.0053,-0.0011,-0.006,1.9e-05,0.0027,0.004,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.00088,0.00088,0.018,0.067,0.067,0.0093,0.071,0.071,0.045,3.3e-05,3.3e-05,6.3e-06,0.037,0.037,0.0005,0,0,0,0,0,0,0,0 -13590000,-0.28,0.012,-0.0057,0.96,0.0059,-0.0046,0.017,0.0066,-0.0035,0.0038,-0.0011,-0.0061,1.8e-05,0.0027,0.004,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.00083,0.00083,0.018,0.054,0.054,0.0088,0.057,0.057,0.044,3.2e-05,3.2e-05,6.3e-06,0.037,0.037,0.0005,0,0,0,0,0,0,0,0 -13690000,-0.28,0.012,-0.0057,0.96,0.0063,-0.0035,0.017,0.0072,-0.004,0.0064,-0.0011,-0.0061,1.8e-05,0.0026,0.004,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.00086,0.00086,0.018,0.062,0.062,0.0085,0.065,0.065,0.044,3.2e-05,3.2e-05,6.3e-06,0.037,0.037,0.0005,0,0,0,0,0,0,0,0 -13790000,-0.28,0.012,-0.0058,0.96,0.0085,-0.002,0.017,0.01,-0.0014,0.0059,-0.0011,-0.0061,1.7e-05,0.0024,0.004,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.00082,0.00082,0.018,0.051,0.051,0.0082,0.054,0.054,0.043,3e-05,3e-05,6.3e-06,0.037,0.037,0.0005,0,0,0,0,0,0,0,0 -13890000,-0.28,0.012,-0.0057,0.96,0.0087,-0.0028,0.018,0.011,-0.0017,0.0081,-0.0011,-0.0061,1.7e-05,0.0024,0.004,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.00084,0.00084,0.018,0.058,0.058,0.008,0.062,0.062,0.042,3e-05,3e-05,6.3e-06,0.037,0.037,0.0005,0,0,0,0,0,0,0,0 -13990000,-0.28,0.012,-0.0057,0.96,0.0038,-0.0069,0.017,0.0084,-0.0028,0.007,-0.0011,-0.0061,1.8e-05,0.0026,0.004,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.00081,0.0008,0.018,0.049,0.049,0.0077,0.052,0.052,0.041,2.9e-05,2.9e-05,6.3e-06,0.037,0.037,0.0005,0,0,0,0,0,0,0,0 -14090000,-0.28,0.012,-0.0057,0.96,0.0061,-0.0023,0.018,0.0087,-0.0033,0.0035,-0.0011,-0.0061,1.8e-05,0.0027,0.0039,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.00083,0.00083,0.018,0.055,0.055,0.0076,0.059,0.059,0.041,2.9e-05,2.9e-05,6.3e-06,0.037,0.037,0.0005,0,0,0,0,0,0,0,0 -14190000,-0.28,0.012,-0.0057,0.96,0.0079,-0.0019,0.018,0.0084,-0.0026,0.0037,-0.0011,-0.0061,1.8e-05,0.0028,0.0038,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.0008,0.0008,0.018,0.047,0.047,0.0074,0.05,0.05,0.04,2.7e-05,2.7e-05,6.3e-06,0.037,0.037,0.0005,0,0,0,0,0,0,0,0 -14290000,-0.28,0.012,-0.0056,0.96,0.0071,-0.0023,0.016,0.0093,-0.0027,0.0079,-0.0011,-0.0061,1.8e-05,0.0027,0.0038,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.00082,0.00082,0.018,0.053,0.053,0.0073,0.058,0.058,0.04,2.7e-05,2.7e-05,6.3e-06,0.037,0.037,0.0005,0,0,0,0,0,0,0,0 -14390000,-0.28,0.012,-0.0056,0.96,0.0057,-0.0022,0.018,0.0086,-0.0035,0.012,-0.0011,-0.0061,1.8e-05,0.0027,0.004,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.00079,0.00079,0.018,0.045,0.045,0.0071,0.049,0.049,0.039,2.6e-05,2.6e-05,6.3e-06,0.037,0.037,0.0005,0,0,0,0,0,0,0,0 -14490000,-0.28,0.012,-0.0058,0.96,0.0081,-0.0024,0.021,0.0094,-0.0038,0.015,-0.0011,-0.0061,1.8e-05,0.0027,0.004,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.00081,0.00081,0.018,0.051,0.051,0.0071,0.056,0.056,0.038,2.6e-05,2.6e-05,6.3e-06,0.037,0.037,0.0005,0,0,0,0,0,0,0,0 -14590000,-0.28,0.012,-0.0058,0.96,0.0023,-0.0046,0.019,0.0059,-0.0044,0.011,-0.0011,-0.0061,1.8e-05,0.0034,0.0037,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.00078,0.00078,0.018,0.044,0.044,0.007,0.048,0.048,0.038,2.5e-05,2.5e-05,6.3e-06,0.036,0.037,0.0005,0,0,0,0,0,0,0,0 -14690000,-0.28,0.012,-0.0058,0.96,0.0034,-0.0049,0.019,0.0061,-0.0049,0.011,-0.0011,-0.0061,1.8e-05,0.0034,0.0037,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.0008,0.0008,0.018,0.049,0.049,0.007,0.055,0.055,0.037,2.5e-05,2.5e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 -14790000,-0.28,0.012,-0.0062,0.96,-0.001,0.00013,0.019,0.0048,0.00036,0.014,-0.001,-0.0061,1.6e-05,0.0029,0.003,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.00077,0.00077,0.018,0.043,0.043,0.0069,0.048,0.048,0.037,2.3e-05,2.3e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 -14890000,-0.28,0.012,-0.0061,0.96,-2.9e-05,0.003,0.023,0.0048,0.00052,0.014,-0.001,-0.0061,1.6e-05,0.0029,0.003,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.00079,0.00079,0.018,0.048,0.048,0.007,0.055,0.055,0.037,2.3e-05,2.3e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 -14990000,-0.28,0.012,-0.0061,0.96,-0.0011,0.00057,0.026,0.0039,-0.0011,0.016,-0.0011,-0.0061,1.7e-05,0.0033,0.0031,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.00076,0.00076,0.018,0.042,0.042,0.0069,0.047,0.047,0.036,2.2e-05,2.2e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 -15090000,-0.28,0.012,-0.0061,0.96,-0.00082,-0.00051,0.03,0.0038,-0.0011,0.019,-0.0011,-0.0061,1.7e-05,0.0033,0.0031,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.00078,0.00078,0.019,0.047,0.047,0.007,0.054,0.054,0.036,2.2e-05,2.2e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 -15190000,-0.28,0.012,-0.0062,0.96,-0.0011,0.00091,0.03,0.0031,-0.00078,0.021,-0.001,-0.0061,1.7e-05,0.0034,0.003,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.00075,0.00075,0.019,0.041,0.041,0.007,0.047,0.047,0.036,2e-05,2e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 -15290000,-0.28,0.012,-0.0063,0.96,-0.0019,0.0018,0.03,0.0029,-0.00068,0.018,-0.001,-0.0061,1.7e-05,0.0037,0.0028,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.00077,0.00077,0.019,0.046,0.046,0.0071,0.054,0.054,0.035,2e-05,2e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 -15390000,-0.28,0.012,-0.0064,0.96,-0.0031,-0.00057,0.029,0.0024,-0.00068,0.018,-0.001,-0.0061,1.7e-05,0.0039,0.0027,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.00074,0.00074,0.019,0.04,0.04,0.0071,0.047,0.047,0.035,1.9e-05,1.9e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 -15490000,-0.28,0.012,-0.0064,0.96,-0.0032,0.002,0.029,0.0021,-0.00063,0.019,-0.001,-0.0061,1.7e-05,0.004,0.0026,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.00076,0.00076,0.019,0.045,0.045,0.0072,0.053,0.053,0.035,1.9e-05,1.9e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 -15590000,-0.28,0.012,-0.0062,0.96,-0.0012,-0.004,0.029,0.0045,-0.0048,0.018,-0.0011,-0.0061,1.9e-05,0.0042,0.0035,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.00073,0.00073,0.019,0.039,0.039,0.0072,0.046,0.046,0.035,1.8e-05,1.8e-05,6.3e-06,0.035,0.035,0.0005,0,0,0,0,0,0,0,0 -15690000,-0.28,0.012,-0.0062,0.96,-0.0034,-0.0018,0.029,0.0042,-0.0051,0.019,-0.0011,-0.0061,1.9e-05,0.0043,0.0034,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.00075,0.00075,0.019,0.044,0.044,0.0073,0.053,0.053,0.034,1.8e-05,1.8e-05,6.3e-06,0.035,0.035,0.0005,0,0,0,0,0,0,0,0 -15790000,-0.28,0.012,-0.0063,0.96,-0.0023,0.00013,0.029,0.0036,-0.004,0.02,-0.0011,-0.0061,1.9e-05,0.0044,0.0033,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.00072,0.00072,0.019,0.038,0.038,0.0073,0.046,0.046,0.034,1.7e-05,1.7e-05,6.3e-06,0.035,0.035,0.0005,0,0,0,0,0,0,0,0 -15890000,-0.28,0.012,-0.0063,0.96,-0.00072,-0.0017,0.03,0.0035,-0.0041,0.02,-0.0011,-0.0061,1.9e-05,0.0046,0.0031,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.00073,0.00073,0.019,0.043,0.043,0.0074,0.053,0.053,0.034,1.7e-05,1.7e-05,6.3e-06,0.035,0.035,0.0005,0,0,0,0,0,0,0,0 -15990000,-0.28,0.012,-0.0062,0.96,-0.00043,-0.0011,0.027,0.0029,-0.0034,0.019,-0.0011,-0.0061,1.8e-05,0.0048,0.0028,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.0007,0.0007,0.019,0.038,0.038,0.0074,0.046,0.046,0.034,1.5e-05,1.5e-05,6.3e-06,0.035,0.035,0.0005,0,0,0,0,0,0,0,0 -16090000,-0.28,0.012,-0.0062,0.96,-5.6e-05,-8.3e-05,0.024,0.0029,-0.0034,0.019,-0.0011,-0.0061,1.8e-05,0.005,0.0026,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.00072,0.00072,0.019,0.042,0.042,0.0076,0.053,0.053,0.034,1.5e-05,1.5e-05,6.3e-06,0.035,0.035,0.0005,0,0,0,0,0,0,0,0 -16190000,-0.28,0.012,-0.0062,0.96,-0.0004,-0.00054,0.023,0.0011,-0.003,0.016,-0.0011,-0.006,1.8e-05,0.0057,0.0021,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.00069,0.00069,0.019,0.037,0.037,0.0076,0.046,0.046,0.034,1.4e-05,1.4e-05,6.3e-06,0.034,0.034,0.0005,0,0,0,0,0,0,0,0 -16290000,-0.28,0.012,-0.0062,0.96,-0.0013,0.00057,0.023,0.001,-0.0029,0.017,-0.0011,-0.006,1.8e-05,0.0057,0.0021,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.0007,0.0007,0.019,0.042,0.042,0.0077,0.052,0.052,0.034,1.4e-05,1.4e-05,6.3e-06,0.034,0.034,0.0005,0,0,0,0,0,0,0,0 -16390000,-0.28,0.012,-0.0062,0.96,-0.0011,0.00095,0.023,0.0022,-0.0025,0.017,-0.0011,-0.0061,1.8e-05,0.0056,0.0021,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.00067,0.00067,0.019,0.037,0.037,0.0077,0.046,0.046,0.034,1.3e-05,1.3e-05,6.3e-06,0.034,0.034,0.0005,0,0,0,0,0,0,0,0 -16490000,-0.28,0.012,-0.0063,0.96,-0.0034,0.0019,0.026,0.002,-0.0023,0.021,-0.0011,-0.0061,1.8e-05,0.0055,0.0022,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.00069,0.00069,0.019,0.041,0.041,0.0079,0.052,0.052,0.034,1.3e-05,1.3e-05,6.3e-06,0.034,0.034,0.0005,0,0,0,0,0,0,0,0 -16590000,-0.28,0.012,-0.0063,0.96,-0.0084,0.0021,0.029,0.0014,-0.002,0.021,-0.0011,-0.0061,1.8e-05,0.0057,0.0021,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.00066,0.00066,0.019,0.036,0.036,0.0079,0.046,0.046,0.034,1.2e-05,1.2e-05,6.3e-06,0.034,0.034,0.0005,0,0,0,0,0,0,0,0 -16690000,-0.28,0.012,-0.0064,0.96,-0.011,0.0061,0.029,0.00043,-0.0016,0.022,-0.0011,-0.0061,1.8e-05,0.0058,0.002,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.00067,0.00067,0.019,0.04,0.04,0.008,0.052,0.052,0.034,1.2e-05,1.2e-05,6.3e-06,0.034,0.034,0.0005,0,0,0,0,0,0,0,0 -16790000,-0.28,0.012,-0.0062,0.96,-0.012,0.0056,0.028,0.00061,-0.0016,0.022,-0.0011,-0.0061,1.9e-05,0.0059,0.0022,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.00064,0.00064,0.019,0.035,0.036,0.008,0.046,0.046,0.034,1.1e-05,1.1e-05,6.3e-06,0.033,0.033,0.0005,0,0,0,0,0,0,0,0 -16890000,-0.28,0.012,-0.0062,0.96,-0.012,0.0061,0.029,-0.00062,-0.001,0.02,-0.0011,-0.0061,1.9e-05,0.0062,0.0019,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.00065,0.00065,0.019,0.04,0.04,0.0082,0.052,0.052,0.034,1.1e-05,1.1e-05,6.3e-06,0.033,0.033,0.0005,0,0,0,0,0,0,0,0 -16990000,-0.28,0.012,-0.0062,0.96,-0.0072,0.0065,0.029,0.0023,-0.0011,0.019,-0.0012,-0.0061,1.9e-05,0.0058,0.0024,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.00063,0.00063,0.019,0.038,0.038,0.0082,0.054,0.054,0.034,1e-05,1e-05,6.3e-06,0.033,0.033,0.0005,0,0,0,0,0,0,0,0 -17090000,-0.28,0.012,-0.0063,0.96,-0.0093,0.0088,0.028,0.0014,-0.00034,0.018,-0.0012,-0.0061,1.9e-05,0.0061,0.0022,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.00064,0.00064,0.019,0.042,0.042,0.0083,0.062,0.062,0.034,1e-05,1e-05,6.3e-06,0.033,0.033,0.0005,0,0,0,0,0,0,0,0 -17190000,-0.28,0.012,-0.0062,0.96,-0.0075,0.0075,0.03,0.0033,-0.0038,0.022,-0.0012,-0.0061,2e-05,0.0061,0.0032,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.00062,0.00062,0.019,0.04,0.04,0.0083,0.064,0.064,0.034,9.4e-06,9.4e-06,6.3e-06,0.033,0.033,0.0005,0,0,0,0,0,0,0,0 -17290000,-0.28,0.011,-0.0062,0.96,-0.0083,0.0073,0.03,0.0025,-0.0031,0.021,-0.0012,-0.0061,2e-05,0.0063,0.0031,-0.14,-0.017,-0.0037,0.57,0,0,0,0,0,0.00063,0.00063,0.019,0.044,0.044,0.0084,0.072,0.072,0.034,9.4e-06,9.4e-06,6.3e-06,0.033,0.033,0.0005,0,0,0,0,0,0,0,0 -17390000,-0.28,0.011,-0.0062,0.96,-0.00072,0.013,0.029,0.0061,-0.0011,0.021,-0.0012,-0.0061,2e-05,0.0059,0.0033,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.0006,0.0006,0.019,0.037,0.037,0.0084,0.058,0.058,0.034,8.5e-06,8.5e-06,6.3e-06,0.032,0.032,0.0005,0,0,0,0,0,0,0,0 -17490000,-0.28,0.011,-0.0062,0.96,0.0014,0.014,0.029,0.0062,0.00024,0.023,-0.0012,-0.0061,2e-05,0.0059,0.0032,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00061,0.00061,0.019,0.041,0.041,0.0085,0.066,0.066,0.034,8.5e-06,8.5e-06,6.3e-06,0.032,0.032,0.0005,0,0,0,0,0,0,0,0 -17590000,-0.28,0.012,-0.0062,0.96,-0.0023,0.011,0.028,0.0018,-0.00062,0.02,-0.0012,-0.0061,2.1e-05,0.0071,0.0028,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00058,0.00058,0.019,0.035,0.035,0.0085,0.055,0.055,0.034,7.7e-06,7.7e-06,6.3e-06,0.032,0.032,0.0005,0,0,0,0,0,0,0,0 -17690000,-0.28,0.011,-0.0063,0.96,-0.0016,0.012,0.029,0.0015,0.00057,0.023,-0.0012,-0.0061,2.1e-05,0.0071,0.0028,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00059,0.00059,0.019,0.038,0.038,0.0086,0.061,0.061,0.034,7.7e-06,7.7e-06,6.3e-06,0.032,0.032,0.0005,0,0,0,0,0,0,0,0 -17790000,-0.28,0.011,-0.0063,0.96,-0.0017,0.013,0.029,0.0029,0.0015,0.028,-0.0012,-0.0061,2.1e-05,0.0068,0.0031,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00057,0.00057,0.019,0.037,0.037,0.0086,0.064,0.064,0.034,7.1e-06,7.1e-06,6.3e-06,0.032,0.032,0.0005,0,0,0,0,0,0,0,0 -17890000,-0.28,0.011,-0.0062,0.96,-0.0041,0.014,0.029,0.0026,0.0028,0.032,-0.0012,-0.0061,2.1e-05,0.0067,0.0032,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00058,0.00058,0.019,0.041,0.041,0.0086,0.071,0.071,0.035,7.1e-06,7.1e-06,6.3e-06,0.032,0.032,0.0005,0,0,0,0,0,0,0,0 -17990000,-0.28,0.011,-0.0063,0.96,-0.0043,0.017,0.029,0.0023,0.0061,0.033,-0.0012,-0.0061,2e-05,0.0065,0.0028,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00055,0.00055,0.019,0.034,0.034,0.0086,0.058,0.058,0.034,6.4e-06,6.4e-06,6.3e-06,0.032,0.032,0.0005,0,0,0,0,0,0,0,0 -18090000,-0.28,0.011,-0.0064,0.96,-0.004,0.018,0.028,0.0018,0.0079,0.031,-0.0012,-0.0061,2e-05,0.0068,0.0026,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00056,0.00056,0.019,0.038,0.038,0.0087,0.065,0.065,0.035,6.4e-06,6.4e-06,6.3e-06,0.032,0.032,0.0005,0,0,0,0,0,0,0,0 -18190000,-0.28,0.011,-0.0063,0.96,-0.0019,0.015,0.028,0.0031,0.0055,0.029,-0.0013,-0.0061,2.1e-05,0.0072,0.0029,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00054,0.00053,0.019,0.032,0.032,0.0086,0.054,0.054,0.035,5.8e-06,5.8e-06,6.3e-06,0.031,0.031,0.0005,0,0,0,0,0,0,0,0 -18290000,-0.28,0.011,-0.0064,0.96,-0.0028,0.016,0.027,0.0028,0.007,0.027,-0.0013,-0.0061,2.1e-05,0.0075,0.0027,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00054,0.00054,0.019,0.036,0.036,0.0087,0.061,0.061,0.035,5.8e-06,5.8e-06,6.3e-06,0.031,0.031,0.0005,0,0,0,0,0,0,0,0 -18390000,-0.28,0.011,-0.0062,0.96,0.00055,0.014,0.027,0.0051,0.0051,0.026,-0.0013,-0.0061,2.2e-05,0.0075,0.0032,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00052,0.00052,0.019,0.031,0.031,0.0086,0.051,0.051,0.035,5.3e-06,5.3e-06,6.3e-06,0.031,0.031,0.0005,0,0,0,0,0,0,0,0 -18490000,-0.28,0.011,-0.0063,0.96,-0.0019,0.014,0.026,0.005,0.0065,0.028,-0.0013,-0.0061,2.2e-05,0.0076,0.0032,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00053,0.00053,0.019,0.034,0.034,0.0087,0.058,0.058,0.035,5.3e-06,5.3e-06,6.3e-06,0.031,0.031,0.0005,0,0,0,0,0,0,0,0 -18590000,-0.28,0.011,-0.0061,0.96,-0.0021,0.012,0.026,0.0041,0.0049,0.031,-0.0013,-0.0061,2.2e-05,0.0078,0.0035,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00051,0.00051,0.019,0.03,0.03,0.0087,0.049,0.049,0.035,4.9e-06,4.9e-06,6.3e-06,0.031,0.031,0.0005,0,0,0,0,0,0,0,0 -18690000,-0.28,0.011,-0.0061,0.96,-0.0018,0.012,0.024,0.0039,0.0061,0.029,-0.0013,-0.0061,2.2e-05,0.0081,0.0033,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00052,0.00051,0.019,0.033,0.033,0.0087,0.055,0.055,0.035,4.9e-06,4.9e-06,6.3e-06,0.031,0.031,0.0005,0,0,0,0,0,0,0,0 -18790000,-0.28,0.011,-0.006,0.96,9.3e-05,0.011,0.024,0.0045,0.0048,0.027,-0.0013,-0.0061,2.3e-05,0.0084,0.0034,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.0005,0.0005,0.019,0.03,0.03,0.0087,0.048,0.048,0.035,4.4e-06,4.4e-06,6.3e-06,0.031,0.031,0.0005,0,0,0,0,0,0,0,0 -18890000,-0.28,0.011,-0.006,0.96,0.0017,0.011,0.021,0.0046,0.006,0.023,-0.0013,-0.0061,2.3e-05,0.0087,0.0031,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.0005,0.0005,0.019,0.033,0.033,0.0087,0.054,0.054,0.035,4.4e-06,4.4e-06,6.3e-06,0.031,0.031,0.0005,0,0,0,0,0,0,0,0 -18990000,-0.28,0.011,-0.0059,0.96,0.0015,0.011,0.022,0.0037,0.0047,0.026,-0.0013,-0.0061,2.3e-05,0.009,0.0034,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00049,0.00048,0.019,0.029,0.029,0.0086,0.047,0.047,0.035,4.1e-06,4.1e-06,6.3e-06,0.03,0.031,0.0005,0,0,0,0,0,0,0,0 -19090000,-0.28,0.011,-0.006,0.96,0.0035,0.012,0.023,0.0039,0.0058,0.022,-0.0013,-0.0061,2.3e-05,0.0093,0.0031,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00049,0.00049,0.019,0.032,0.032,0.0087,0.052,0.053,0.036,4.1e-06,4.1e-06,6.3e-06,0.03,0.031,0.0005,0,0,0,0,0,0,0,0 -19190000,-0.28,0.011,-0.0059,0.96,0.0036,0.011,0.022,0.0031,0.0046,0.022,-0.0013,-0.0061,2.4e-05,0.0097,0.0032,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00048,0.00047,0.019,0.028,0.028,0.0086,0.046,0.046,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.28,0.011,-0.0059,0.96,0.0046,0.011,0.023,0.0035,0.0057,0.021,-0.0013,-0.0061,2.4e-05,0.0099,0.003,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00048,0.00048,0.019,0.031,0.031,0.0087,0.052,0.052,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.28,0.011,-0.006,0.96,0.0043,0.0095,0.024,0.0027,0.0056,0.02,-0.0013,-0.0061,2.4e-05,0.01,0.0028,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00047,0.00046,0.019,0.028,0.028,0.0086,0.045,0.045,0.036,3.4e-06,3.4e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -19490000,-0.28,0.011,-0.006,0.96,0.0051,0.0099,0.023,0.0032,0.0066,0.019,-0.0013,-0.0061,2.4e-05,0.01,0.0027,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00047,0.00047,0.019,0.031,0.031,0.0087,0.051,0.051,0.036,3.4e-06,3.4e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -19590000,-0.28,0.011,-0.0059,0.96,0.0071,0.0099,0.025,0.0038,0.0042,0.019,-0.0014,-0.0061,2.4e-05,0.011,0.003,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00046,0.00045,0.019,0.027,0.027,0.0086,0.045,0.045,0.036,3.1e-06,3.1e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -19690000,-0.28,0.011,-0.0059,0.96,0.0092,0.0089,0.023,0.0046,0.0052,0.019,-0.0014,-0.0061,2.4e-05,0.011,0.0029,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00046,0.00046,0.019,0.03,0.03,0.0086,0.05,0.05,0.036,3.1e-06,3.1e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -19790000,-0.28,0.011,-0.006,0.96,0.012,0.0078,0.022,0.0056,0.0051,0.015,-0.0014,-0.0061,2.4e-05,0.011,0.0028,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00045,0.00045,0.019,0.029,0.029,0.0086,0.053,0.053,0.036,2.9e-06,2.9e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -19890000,-0.28,0.011,-0.006,0.96,0.012,0.0078,0.022,0.0069,0.0058,0.013,-0.0014,-0.0061,2.4e-05,0.011,0.0026,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00046,0.00045,0.019,0.032,0.032,0.0086,0.059,0.059,0.036,2.9e-06,2.9e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -19990000,-0.28,0.012,-0.006,0.96,0.012,0.0092,0.019,0.0064,0.0058,0.0098,-0.0014,-0.0061,2.5e-05,0.012,0.0023,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00044,0.00044,0.019,0.028,0.028,0.0085,0.05,0.05,0.036,2.6e-06,2.6e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -20090000,-0.28,0.012,-0.006,0.96,0.011,0.012,0.019,0.0076,0.0069,0.013,-0.0014,-0.0061,2.5e-05,0.012,0.0024,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00045,0.00044,0.019,0.03,0.03,0.0086,0.056,0.056,0.036,2.6e-06,2.6e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -20190000,-0.28,0.012,-0.006,0.96,0.011,0.0085,0.02,0.0073,0.0065,0.013,-0.0014,-0.006,2.5e-05,0.012,0.0023,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00044,0.00043,0.019,0.03,0.03,0.0085,0.059,0.059,0.036,2.5e-06,2.5e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -20290000,-0.28,0.012,-0.006,0.96,0.014,0.01,0.02,0.0085,0.0074,0.013,-0.0014,-0.006,2.5e-05,0.012,0.0023,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00044,0.00044,0.019,0.032,0.032,0.0085,0.066,0.066,0.036,2.5e-06,2.5e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -20390000,-0.28,0.012,-0.0059,0.96,0.014,0.0089,0.021,0.0081,0.0069,0.014,-0.0014,-0.006,2.5e-05,0.013,0.0023,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00043,0.00043,0.019,0.031,0.031,0.0084,0.068,0.068,0.036,2.3e-06,2.3e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -20490000,-0.28,0.012,-0.006,0.96,0.019,0.011,0.02,0.0097,0.0079,0.013,-0.0014,-0.006,2.5e-05,0.013,0.0021,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00044,0.00043,0.019,0.034,0.034,0.0085,0.076,0.076,0.036,2.3e-06,2.3e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -20590000,-0.28,0.012,-0.0059,0.96,0.017,0.011,0.02,0.0086,0.0073,0.011,-0.0014,-0.006,2.5e-05,0.013,0.0019,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00043,0.00042,0.019,0.032,0.032,0.0084,0.077,0.077,0.036,2.2e-06,2.2e-06,6.3e-06,0.029,0.03,0.0005,0,0,0,0,0,0,0,0 -20690000,-0.28,0.012,-0.0058,0.96,0.019,0.011,0.021,0.01,0.0083,0.011,-0.0014,-0.006,2.5e-05,0.013,0.0019,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00043,0.00043,0.019,0.035,0.035,0.0084,0.086,0.086,0.036,2.2e-06,2.2e-06,6.3e-06,0.029,0.03,0.0005,0,0,0,0,0,0,0,0 -20790000,-0.28,0.012,-0.0052,0.96,0.019,0.0068,0.0055,0.0065,0.0055,0.0097,-0.0014,-0.006,2.6e-05,0.014,0.0017,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00042,0.00041,0.019,0.028,0.028,0.0084,0.066,0.066,0.036,1.9e-06,1.9e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -20890000,-0.28,0.0069,0.0034,0.96,0.028,-0.0036,-0.11,0.0089,0.0056,0.0034,-0.0014,-0.006,2.6e-05,0.014,0.0017,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00042,0.00041,0.019,0.031,0.031,0.0084,0.074,0.074,0.036,1.9e-06,1.9e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -20990000,-0.28,0.0032,0.0064,0.96,0.04,-0.02,-0.25,0.0062,0.0039,-0.011,-0.0013,-0.006,2.6e-05,0.014,0.0015,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.0004,0.0004,0.019,0.026,0.026,0.0083,0.06,0.06,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.28,0.0037,0.0049,0.96,0.056,-0.033,-0.37,0.011,0.0012,-0.042,-0.0013,-0.006,2.6e-05,0.014,0.0015,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.0004,0.0004,0.019,0.029,0.029,0.0084,0.066,0.066,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.28,0.0057,0.0022,0.96,0.059,-0.036,-0.5,0.0072,0.0019,-0.078,-0.0013,-0.006,2.5e-05,0.015,0.00075,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00039,0.00039,0.019,0.025,0.025,0.0083,0.055,0.055,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.28,0.0072,0.00015,0.96,0.059,-0.04,-0.63,0.013,-0.002,-0.14,-0.0013,-0.006,2.5e-05,0.015,0.00069,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.0004,0.00039,0.019,0.028,0.028,0.0083,0.061,0.061,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.28,0.0081,-0.0015,0.96,0.05,-0.03,-0.75,0.0093,0.0039,-0.2,-0.0012,-0.0059,2.3e-05,0.015,-0.0013,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00039,0.00038,0.019,0.027,0.027,0.0082,0.063,0.063,0.036,1.5e-06,1.5e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -21490000,-0.29,0.0086,-0.0022,0.96,0.045,-0.028,-0.89,0.014,0.0011,-0.29,-0.0012,-0.0059,2.3e-05,0.015,-0.0014,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00039,0.00039,0.019,0.03,0.03,0.0083,0.07,0.07,0.036,1.5e-06,1.5e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -21590000,-0.29,0.0088,-0.003,0.96,0.029,-0.017,-1,0.0067,0.0091,-0.38,-0.0012,-0.0059,2.2e-05,0.015,-0.0036,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00038,0.00038,0.019,0.029,0.029,0.0082,0.072,0.072,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 -21690000,-0.29,0.0087,-0.0033,0.96,0.026,-0.013,-1.1,0.0094,0.0076,-0.49,-0.0012,-0.0059,2.2e-05,0.015,-0.0038,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00039,0.00038,0.019,0.032,0.032,0.0083,0.08,0.08,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 -21790000,-0.29,0.0089,-0.0039,0.96,0.017,-0.00075,-1.3,0.0048,0.018,-0.61,-0.0011,-0.0059,2e-05,0.015,-0.0058,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00038,0.00037,0.02,0.031,0.031,0.0082,0.081,0.081,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 -21890000,-0.29,0.0091,-0.0042,0.96,0.012,0.0037,-1.4,0.0062,0.018,-0.75,-0.0011,-0.0059,2e-05,0.015,-0.0059,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00038,0.00038,0.02,0.033,0.033,0.0082,0.09,0.09,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 -21990000,-0.29,0.0096,-0.0051,0.96,0.0013,0.014,-1.4,-0.0047,0.028,-0.89,-0.0011,-0.0059,1.9e-05,0.016,-0.008,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00037,0.00037,0.02,0.031,0.031,0.0082,0.091,0.091,0.036,1.3e-06,1.3e-06,6.3e-06,0.028,0.029,0.0005,0,0,0,0,0,0,0,0 -22090000,-0.29,0.01,-0.0057,0.96,-0.0024,0.019,-1.4,-0.0048,0.029,-1,-0.0011,-0.0059,1.9e-05,0.016,-0.0081,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00037,0.00037,0.02,0.034,0.034,0.0082,0.1,0.1,0.036,1.3e-06,1.3e-06,6.3e-06,0.028,0.029,0.0005,0,0,0,0,0,0,0,0 -22190000,-0.29,0.011,-0.0063,0.96,-0.015,0.027,-1.4,-0.014,0.039,-1.2,-0.0011,-0.0059,1.8e-05,0.016,-0.0095,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00037,0.00036,0.02,0.032,0.032,0.0081,0.1,0.1,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 -22290000,-0.29,0.011,-0.007,0.96,-0.021,0.032,-1.4,-0.016,0.042,-1.3,-0.0011,-0.0059,1.8e-05,0.017,-0.0096,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00037,0.00036,0.02,0.034,0.034,0.0081,0.11,0.11,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 -22390000,-0.29,0.012,-0.0073,0.96,-0.031,0.037,-1.4,-0.025,0.045,-1.5,-0.001,-0.0059,1.8e-05,0.017,-0.01,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00036,0.00036,0.02,0.032,0.032,0.0081,0.11,0.11,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 -22490000,-0.29,0.012,-0.0074,0.96,-0.037,0.043,-1.4,-0.029,0.049,-1.6,-0.001,-0.0059,1.8e-05,0.017,-0.01,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00036,0.00036,0.02,0.034,0.034,0.0081,0.12,0.12,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 -22590000,-0.29,0.013,-0.0072,0.96,-0.042,0.049,-1.4,-0.029,0.053,-1.7,-0.0011,-0.0059,1.9e-05,0.017,-0.0099,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00036,0.00035,0.02,0.032,0.032,0.0081,0.12,0.12,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 -22690000,-0.29,0.013,-0.0071,0.96,-0.046,0.054,-1.4,-0.034,0.058,-1.9,-0.0011,-0.0059,1.9e-05,0.017,-0.01,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00036,0.00035,0.02,0.034,0.034,0.0081,0.13,0.13,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 -22790000,-0.29,0.013,-0.007,0.96,-0.052,0.057,-1.4,-0.039,0.058,-2,-0.0011,-0.0059,1.9e-05,0.018,-0.0098,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00035,0.02,0.032,0.032,0.0081,0.13,0.13,0.036,1e-06,1e-06,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -22890000,-0.28,0.014,-0.0072,0.96,-0.058,0.063,-1.4,-0.045,0.064,-2.2,-0.0011,-0.0059,1.9e-05,0.018,-0.0099,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00035,0.02,0.034,0.034,0.0081,0.14,0.14,0.036,1e-06,1e-06,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -22990000,-0.28,0.014,-0.007,0.96,-0.062,0.061,-1.4,-0.049,0.061,-2.3,-0.0011,-0.0059,1.9e-05,0.019,-0.0094,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.02,0.032,0.032,0.0081,0.14,0.14,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 -23090000,-0.28,0.014,-0.007,0.96,-0.068,0.065,-1.4,-0.056,0.068,-2.5,-0.0011,-0.0059,1.9e-05,0.019,-0.0095,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.02,0.034,0.034,0.0081,0.15,0.15,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 -23190000,-0.28,0.014,-0.0068,0.96,-0.069,0.057,-1.4,-0.054,0.06,-2.6,-0.0011,-0.0059,1.9e-05,0.019,-0.0085,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.02,0.032,0.032,0.008,0.15,0.15,0.035,9e-07,8.9e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -23290000,-0.28,0.015,-0.0072,0.96,-0.075,0.062,-1.4,-0.061,0.066,-2.8,-0.0011,-0.0059,1.9e-05,0.019,-0.0086,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.02,0.034,0.034,0.0081,0.16,0.16,0.036,9e-07,8.9e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -23390000,-0.28,0.015,-0.0071,0.96,-0.073,0.062,-1.4,-0.054,0.065,-2.9,-0.0011,-0.0059,2e-05,0.018,-0.0079,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00034,0.02,0.031,0.031,0.008,0.16,0.16,0.036,8.5e-07,8.5e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -23490000,-0.28,0.015,-0.0072,0.96,-0.078,0.064,-1.4,-0.062,0.071,-3,-0.0011,-0.0059,2e-05,0.019,-0.0079,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.02,0.033,0.033,0.0081,0.17,0.17,0.036,8.5e-07,8.5e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -23590000,-0.28,0.015,-0.0072,0.96,-0.076,0.055,-1.4,-0.056,0.057,-3.2,-0.0012,-0.0059,2e-05,0.019,-0.0069,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00033,0.02,0.031,0.031,0.008,0.17,0.17,0.035,8.1e-07,8.1e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -23690000,-0.28,0.015,-0.0078,0.96,-0.075,0.058,-1.3,-0.064,0.063,-3.3,-0.0012,-0.0059,2e-05,0.019,-0.0069,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00034,0.02,0.033,0.033,0.0081,0.18,0.18,0.036,8.1e-07,8.1e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -23790000,-0.28,0.018,-0.0093,0.96,-0.06,0.054,-0.96,-0.051,0.057,-3.4,-0.0012,-0.0059,2.1e-05,0.019,-0.0063,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.02,0.03,0.03,0.008,0.18,0.18,0.035,7.7e-07,7.7e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -23890000,-0.28,0.023,-0.012,0.96,-0.055,0.055,-0.52,-0.056,0.063,-3.5,-0.0012,-0.0059,2.1e-05,0.019,-0.0063,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00036,0.00035,0.02,0.031,0.031,0.008,0.19,0.19,0.035,7.8e-07,7.7e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -23990000,-0.28,0.025,-0.014,0.96,-0.048,0.051,-0.14,-0.044,0.055,-3.6,-0.0012,-0.0059,2.2e-05,0.019,-0.0065,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00036,0.00035,0.02,0.029,0.029,0.008,0.19,0.19,0.036,7.4e-07,7.4e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -24090000,-0.28,0.024,-0.014,0.96,-0.056,0.058,0.09,-0.049,0.061,-3.6,-0.0012,-0.0059,2.2e-05,0.019,-0.0065,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00036,0.00035,0.02,0.03,0.03,0.0081,0.2,0.2,0.036,7.4e-07,7.4e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -24190000,-0.28,0.02,-0.011,0.96,-0.059,0.053,0.077,-0.035,0.045,-3.6,-0.0013,-0.0059,2.3e-05,0.02,-0.0075,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.02,0.029,0.029,0.008,0.2,0.2,0.035,7.2e-07,7.1e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -24290000,-0.28,0.016,-0.009,0.96,-0.063,0.056,0.055,-0.041,0.051,-3.6,-0.0013,-0.0059,2.3e-05,0.02,-0.0076,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00034,0.02,0.03,0.03,0.0081,0.21,0.21,0.036,7.2e-07,7.1e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -24390000,-0.28,0.015,-0.0082,0.96,-0.045,0.049,0.071,-0.02,0.04,-3.6,-0.0013,-0.0059,2.4e-05,0.02,-0.0082,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00034,0.02,0.029,0.029,0.008,0.21,0.21,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 -24490000,-0.28,0.015,-0.0084,0.96,-0.039,0.047,0.069,-0.024,0.045,-3.6,-0.0013,-0.0059,2.4e-05,0.02,-0.0082,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00034,0.02,0.03,0.03,0.008,0.22,0.22,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 -24590000,-0.28,0.015,-0.009,0.96,-0.026,0.044,0.065,-0.0029,0.038,-3.6,-0.0013,-0.006,2.5e-05,0.021,-0.0087,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00034,0.02,0.029,0.029,0.008,0.22,0.22,0.036,6.7e-07,6.6e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -24690000,-0.28,0.015,-0.0096,0.96,-0.023,0.044,0.064,-0.0054,0.043,-3.6,-0.0013,-0.006,2.5e-05,0.021,-0.0087,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00034,0.02,0.03,0.03,0.0081,0.23,0.23,0.036,6.7e-07,6.6e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -24790000,-0.28,0.015,-0.0096,0.96,-0.015,0.041,0.055,0.0081,0.034,-3.6,-0.0014,-0.006,2.6e-05,0.021,-0.0091,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00034,0.02,0.029,0.029,0.008,0.23,0.23,0.035,6.4e-07,6.4e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -24890000,-0.28,0.014,-0.0095,0.96,-0.013,0.045,0.045,0.0067,0.038,-3.6,-0.0014,-0.006,2.6e-05,0.021,-0.0091,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00034,0.02,0.03,0.03,0.008,0.24,0.24,0.035,6.4e-07,6.4e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -24990000,-0.28,0.014,-0.0093,0.96,2.1e-05,0.045,0.038,0.022,0.031,-3.6,-0.0014,-0.006,2.7e-05,0.021,-0.0093,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00034,0.02,0.029,0.029,0.008,0.23,0.23,0.035,6.2e-07,6.2e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -25090000,-0.28,0.014,-0.0096,0.96,0.0046,0.045,0.035,0.022,0.036,-3.6,-0.0014,-0.006,2.7e-05,0.021,-0.0094,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00034,0.02,0.03,0.03,0.0081,0.25,0.25,0.035,6.2e-07,6.2e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -25190000,-0.28,0.014,-0.0098,0.96,0.015,0.039,0.035,0.037,0.023,-3.6,-0.0014,-0.006,2.8e-05,0.021,-0.0097,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00034,0.02,0.028,0.028,0.008,0.24,0.24,0.035,6e-07,6e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -25290000,-0.28,0.013,-0.01,0.96,0.02,0.042,0.029,0.038,0.027,-3.6,-0.0014,-0.006,2.8e-05,0.021,-0.0097,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00034,0.02,0.03,0.03,0.0081,0.26,0.26,0.036,6e-07,6e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -25390000,-0.28,0.013,-0.01,0.96,0.028,0.04,0.028,0.046,0.021,-3.6,-0.0014,-0.006,2.9e-05,0.021,-0.0098,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00034,0.02,0.028,0.028,0.008,0.25,0.25,0.035,5.8e-07,5.8e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -25490000,-0.28,0.013,-0.01,0.96,0.035,0.041,0.027,0.049,0.025,-3.6,-0.0014,-0.006,2.9e-05,0.021,-0.0099,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00034,0.02,0.03,0.03,0.0081,0.27,0.27,0.035,5.8e-07,5.8e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -25590000,-0.28,0.012,-0.01,0.96,0.039,0.034,0.028,0.054,0.008,-3.6,-0.0014,-0.006,3.1e-05,0.021,-0.01,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00034,0.02,0.028,0.028,0.008,0.26,0.26,0.035,5.6e-07,5.6e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -25690000,-0.28,0.012,-0.0098,0.96,0.041,0.034,0.017,0.058,0.011,-3.6,-0.0014,-0.006,3.1e-05,0.021,-0.01,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00034,0.02,0.03,0.03,0.0081,0.28,0.28,0.035,5.7e-07,5.6e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -25790000,-0.28,0.011,-0.0096,0.96,0.05,0.028,0.017,0.062,0.00079,-3.6,-0.0015,-0.006,3.2e-05,0.021,-0.01,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00034,0.02,0.028,0.028,0.008,0.27,0.27,0.035,5.5e-07,5.4e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -25890000,-0.28,0.011,-0.0097,0.96,0.057,0.027,0.019,0.067,0.0035,-3.6,-0.0015,-0.006,3.2e-05,0.021,-0.01,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00034,0.02,0.03,0.03,0.0081,0.28,0.28,0.036,5.5e-07,5.5e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -25990000,-0.28,0.012,-0.0097,0.96,0.056,0.021,0.013,0.058,-0.0086,-3.6,-0.0015,-0.0059,3.3e-05,0.021,-0.01,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00034,0.02,0.028,0.028,0.008,0.28,0.28,0.035,5.3e-07,5.3e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -26090000,-0.28,0.012,-0.0094,0.96,0.062,0.022,0.011,0.064,-0.0065,-3.6,-0.0015,-0.0059,3.3e-05,0.022,-0.01,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00034,0.02,0.03,0.03,0.0081,0.29,0.29,0.035,5.3e-07,5.3e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -26190000,-0.28,0.012,-0.0092,0.96,0.063,0.012,0.0062,0.062,-0.023,-3.6,-0.0015,-0.0059,3.5e-05,0.022,-0.01,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00034,0.02,0.028,0.028,0.008,0.29,0.29,0.035,5.2e-07,5.1e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -26290000,-0.28,0.013,-0.0092,0.96,0.065,0.011,0.00044,0.068,-0.022,-3.6,-0.0015,-0.0059,3.5e-05,0.022,-0.01,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00034,0.02,0.029,0.029,0.0081,0.3,0.3,0.036,5.2e-07,5.2e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -26390000,-0.28,0.013,-0.0086,0.96,0.06,0.00059,0.0044,0.055,-0.037,-3.6,-0.0015,-0.0059,3.6e-05,0.022,-0.01,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00034,0.021,0.028,0.028,0.008,0.3,0.3,0.035,5e-07,5e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -26490000,-0.28,0.012,-0.0084,0.96,0.064,-0.002,0.014,0.061,-0.037,-3.6,-0.0015,-0.0059,3.6e-05,0.022,-0.01,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00034,0.021,0.029,0.029,0.0081,0.31,0.31,0.035,5.1e-07,5e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -26590000,-0.28,0.013,-0.0078,0.96,0.06,-0.013,0.014,0.056,-0.05,-3.6,-0.0015,-0.0059,3.8e-05,0.022,-0.011,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00034,0.021,0.028,0.028,0.008,0.31,0.31,0.035,4.9e-07,4.9e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -26690000,-0.28,0.012,-0.0077,0.96,0.063,-0.018,0.012,0.062,-0.052,-3.6,-0.0015,-0.0059,3.8e-05,0.022,-0.011,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00034,0.021,0.029,0.029,0.0081,0.32,0.32,0.035,4.9e-07,4.9e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -26790000,-0.28,0.012,-0.0075,0.96,0.063,-0.025,0.012,0.055,-0.065,-3.6,-0.0015,-0.0059,3.9e-05,0.022,-0.011,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00034,0.021,0.027,0.027,0.008,0.31,0.31,0.035,4.8e-07,4.7e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -26890000,-0.28,0.012,-0.0068,0.96,0.069,-0.027,0.0069,0.061,-0.067,-3.6,-0.0015,-0.0059,3.9e-05,0.022,-0.011,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00034,0.021,0.029,0.029,0.0081,0.33,0.33,0.036,4.8e-07,4.8e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -26990000,-0.28,0.013,-0.0062,0.96,0.067,-0.034,0.006,0.05,-0.073,-3.6,-0.0015,-0.0059,4e-05,0.022,-0.011,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00034,0.021,0.027,0.027,0.008,0.32,0.32,0.035,4.7e-07,4.6e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -27090000,-0.28,0.013,-0.0061,0.96,0.07,-0.041,0.0088,0.057,-0.077,-3.6,-0.0015,-0.0059,4e-05,0.022,-0.011,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00034,0.021,0.029,0.029,0.0081,0.34,0.34,0.035,4.7e-07,4.6e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -27190000,-0.28,0.013,-0.0061,0.96,0.067,-0.044,0.011,0.043,-0.079,-3.6,-0.0015,-0.0058,4.1e-05,0.022,-0.011,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00033,0.021,0.027,0.027,0.008,0.33,0.33,0.035,4.6e-07,4.5e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -27290000,-0.28,0.014,-0.0062,0.96,0.075,-0.049,0.12,0.05,-0.083,-3.6,-0.0015,-0.0058,4.1e-05,0.022,-0.011,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00034,0.021,0.028,0.028,0.0081,0.35,0.35,0.035,4.6e-07,4.5e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -27390000,-0.28,0.017,-0.0075,0.96,0.071,-0.04,0.45,0.015,-0.029,-3.5,-0.0014,-0.0058,4e-05,0.022,-0.012,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.021,0.02,0.02,0.008,0.15,0.15,0.035,4.4e-07,4.4e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -27490000,-0.28,0.018,-0.0087,0.96,0.075,-0.044,0.76,0.022,-0.033,-3.5,-0.0014,-0.0058,4e-05,0.022,-0.012,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.021,0.021,0.021,0.008,0.16,0.16,0.035,4.4e-07,4.4e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -27590000,-0.28,0.017,-0.0087,0.96,0.063,-0.046,0.85,0.011,-0.022,-3.4,-0.0014,-0.0058,4.1e-05,0.022,-0.013,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.021,0.018,0.018,0.008,0.1,0.1,0.035,4.3e-07,4.3e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -27690000,-0.28,0.014,-0.0078,0.96,0.058,-0.043,0.76,0.017,-0.026,-3.3,-0.0014,-0.0058,4.1e-05,0.023,-0.013,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00034,0.021,0.019,0.019,0.0081,0.1,0.1,0.035,4.4e-07,4.3e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -27790000,-0.28,0.013,-0.0066,0.96,0.053,-0.04,0.75,0.012,-0.02,-3.3,-0.0014,-0.0058,4.1e-05,0.022,-0.013,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00033,0.021,0.018,0.018,0.008,0.076,0.076,0.035,4.3e-07,4.2e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -27890000,-0.28,0.013,-0.0063,0.96,0.061,-0.046,0.79,0.018,-0.025,-3.2,-0.0014,-0.0058,4.1e-05,0.022,-0.013,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00033,0.021,0.019,0.019,0.0081,0.08,0.08,0.036,4.3e-07,4.2e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -27990000,-0.28,0.013,-0.0067,0.96,0.06,-0.048,0.78,0.02,-0.027,-3.1,-0.0014,-0.0058,4.1e-05,0.022,-0.013,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00034,0.021,0.019,0.019,0.008,0.082,0.082,0.035,4.2e-07,4.1e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -28090000,-0.28,0.013,-0.0069,0.96,0.063,-0.048,0.78,0.026,-0.032,-3,-0.0014,-0.0058,4.1e-05,0.022,-0.013,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00034,0.021,0.02,0.02,0.008,0.086,0.086,0.035,4.2e-07,4.2e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -28190000,-0.28,0.014,-0.0064,0.96,0.059,-0.046,0.79,0.026,-0.035,-3,-0.0014,-0.0058,4.1e-05,0.022,-0.012,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00034,0.021,0.02,0.02,0.008,0.089,0.089,0.035,4.1e-07,4.1e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -28290000,-0.28,0.014,-0.0058,0.96,0.064,-0.049,0.79,0.033,-0.039,-2.9,-0.0014,-0.0058,4.1e-05,0.022,-0.012,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.021,0.021,0.021,0.0081,0.093,0.093,0.035,4.1e-07,4.1e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -28390000,-0.28,0.014,-0.0058,0.96,0.064,-0.05,0.79,0.034,-0.04,-2.8,-0.0014,-0.0058,4.1e-05,0.022,-0.012,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.021,0.021,0.021,0.008,0.096,0.096,0.035,4.1e-07,4e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -28490000,-0.28,0.015,-0.0061,0.96,0.065,-0.053,0.79,0.04,-0.045,-2.8,-0.0014,-0.0058,4.1e-05,0.022,-0.012,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.021,0.022,0.022,0.0081,0.1,0.1,0.036,4.1e-07,4e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -28590000,-0.28,0.015,-0.0061,0.96,0.057,-0.053,0.79,0.039,-0.047,-2.7,-0.0014,-0.0058,4.1e-05,0.022,-0.012,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.021,0.021,0.021,0.008,0.1,0.1,0.035,4e-07,3.9e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -28690000,-0.28,0.014,-0.0059,0.96,0.056,-0.054,0.79,0.045,-0.053,-2.6,-0.0014,-0.0058,4.1e-05,0.022,-0.012,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.021,0.023,0.023,0.008,0.11,0.11,0.035,4e-07,3.9e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -28790000,-0.28,0.014,-0.0053,0.96,0.053,-0.052,0.79,0.045,-0.05,-2.5,-0.0014,-0.0058,4e-05,0.022,-0.012,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.021,0.022,0.022,0.008,0.11,0.11,0.035,3.9e-07,3.9e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -28890000,-0.28,0.014,-0.0052,0.96,0.056,-0.053,0.78,0.05,-0.056,-2.5,-0.0014,-0.0058,4e-05,0.022,-0.013,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.021,0.023,0.023,0.0081,0.12,0.12,0.036,3.9e-07,3.9e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -28990000,-0.28,0.014,-0.0049,0.96,0.053,-0.05,0.78,0.05,-0.055,-2.4,-0.0013,-0.0058,4e-05,0.022,-0.013,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.021,0.023,0.023,0.008,0.12,0.12,0.035,3.8e-07,3.8e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -29090000,-0.28,0.014,-0.0047,0.96,0.055,-0.052,0.78,0.055,-0.06,-2.3,-0.0013,-0.0058,4e-05,0.022,-0.013,-0.12,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.021,0.024,0.024,0.008,0.13,0.13,0.035,3.8e-07,3.8e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -29190000,-0.28,0.014,-0.0047,0.96,0.055,-0.05,0.78,0.056,-0.058,-2.2,-0.0013,-0.0058,3.9e-05,0.022,-0.013,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.021,0.023,0.023,0.008,0.13,0.13,0.035,3.8e-07,3.7e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -29290000,-0.28,0.014,-0.0049,0.96,0.057,-0.056,0.78,0.061,-0.064,-2.2,-0.0013,-0.0058,3.9e-05,0.022,-0.013,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.021,0.025,0.024,0.008,0.14,0.14,0.035,3.8e-07,3.7e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -29390000,-0.28,0.014,-0.0055,0.96,0.053,-0.052,0.78,0.058,-0.059,-2.1,-0.0013,-0.0058,3.8e-05,0.022,-0.013,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.021,0.024,0.024,0.008,0.14,0.14,0.035,3.7e-07,3.6e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -29490000,-0.28,0.013,-0.0055,0.96,0.055,-0.052,0.78,0.063,-0.064,-2,-0.0013,-0.0058,3.8e-05,0.022,-0.013,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.021,0.025,0.025,0.008,0.15,0.15,0.036,3.7e-07,3.7e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -29590000,-0.28,0.013,-0.0054,0.96,0.051,-0.049,0.78,0.06,-0.061,-1.9,-0.0013,-0.0058,3.8e-05,0.022,-0.013,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.021,0.024,0.024,0.008,0.15,0.15,0.035,3.6e-07,3.6e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -29690000,-0.28,0.013,-0.0055,0.96,0.054,-0.047,0.78,0.065,-0.066,-1.9,-0.0013,-0.0058,3.8e-05,0.023,-0.013,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.021,0.025,0.025,0.008,0.16,0.16,0.035,3.6e-07,3.6e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -29790000,-0.28,0.014,-0.0052,0.96,0.051,-0.04,0.78,0.061,-0.061,-1.8,-0.0013,-0.0058,3.7e-05,0.023,-0.013,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.021,0.024,0.024,0.008,0.16,0.16,0.035,3.6e-07,3.5e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -29890000,-0.28,0.014,-0.0047,0.96,0.051,-0.041,0.77,0.066,-0.065,-1.7,-0.0013,-0.0058,3.7e-05,0.023,-0.014,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.021,0.026,0.026,0.008,0.17,0.17,0.035,3.6e-07,3.5e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -29990000,-0.28,0.014,-0.0048,0.96,0.046,-0.038,0.77,0.061,-0.064,-1.6,-0.0013,-0.0058,3.7e-05,0.023,-0.014,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.021,0.024,0.024,0.008,0.17,0.17,0.035,3.5e-07,3.5e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -30090000,-0.28,0.014,-0.0049,0.96,0.046,-0.039,0.77,0.065,-0.067,-1.6,-0.0013,-0.0058,3.7e-05,0.023,-0.014,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.021,0.026,0.026,0.008,0.18,0.18,0.035,3.5e-07,3.5e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -30190000,-0.28,0.014,-0.005,0.96,0.041,-0.033,0.77,0.059,-0.058,-1.5,-0.0013,-0.0058,3.6e-05,0.023,-0.014,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.021,0.025,0.025,0.008,0.18,0.18,0.035,3.4e-07,3.4e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -30290000,-0.28,0.014,-0.005,0.96,0.039,-0.033,0.77,0.063,-0.062,-1.4,-0.0013,-0.0058,3.6e-05,0.023,-0.014,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.021,0.026,0.026,0.008,0.19,0.19,0.035,3.5e-07,3.4e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -30390000,-0.28,0.014,-0.005,0.96,0.036,-0.026,0.76,0.062,-0.055,-1.4,-0.0013,-0.0058,3.5e-05,0.023,-0.015,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.021,0.025,0.025,0.0079,0.19,0.19,0.035,3.4e-07,3.3e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -30490000,-0.28,0.014,-0.005,0.96,0.038,-0.026,0.77,0.065,-0.057,-1.3,-0.0013,-0.0058,3.5e-05,0.023,-0.015,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.022,0.026,0.026,0.008,0.2,0.2,0.035,3.4e-07,3.4e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -30590000,-0.28,0.014,-0.0053,0.96,0.037,-0.023,0.76,0.062,-0.053,-1.2,-0.0012,-0.0058,3.5e-05,0.023,-0.015,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.022,0.025,0.025,0.008,0.2,0.2,0.035,3.3e-07,3.3e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -30690000,-0.28,0.015,-0.0056,0.96,0.034,-0.022,0.76,0.065,-0.055,-1.1,-0.0012,-0.0058,3.5e-05,0.023,-0.015,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.022,0.026,0.026,0.008,0.21,0.21,0.035,3.3e-07,3.3e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -30790000,-0.28,0.014,-0.0054,0.96,0.027,-0.012,0.76,0.057,-0.042,-1.1,-0.0012,-0.0058,3.4e-05,0.023,-0.016,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.022,0.025,0.025,0.008,0.21,0.21,0.035,3.3e-07,3.2e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -30890000,-0.28,0.014,-0.0048,0.96,0.026,-0.0086,0.76,0.06,-0.043,-1,-0.0012,-0.0058,3.4e-05,0.023,-0.016,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.022,0.026,0.026,0.008,0.22,0.22,0.035,3.3e-07,3.2e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -30990000,-0.28,0.014,-0.0049,0.96,0.023,-0.0072,0.76,0.057,-0.042,-0.94,-0.0012,-0.0058,3.4e-05,0.024,-0.016,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.022,0.025,0.025,0.0079,0.22,0.22,0.035,3.2e-07,3.2e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -31090000,-0.28,0.014,-0.0051,0.96,0.022,-0.0062,0.76,0.059,-0.043,-0.86,-0.0012,-0.0058,3.4e-05,0.024,-0.016,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.022,0.026,0.026,0.008,0.23,0.23,0.036,3.2e-07,3.2e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -31190000,-0.28,0.014,-0.0052,0.96,0.019,-0.0019,0.76,0.055,-0.038,-0.79,-0.0012,-0.0058,3.4e-05,0.024,-0.016,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.022,0.025,0.025,0.008,0.23,0.23,0.035,3.2e-07,3.1e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -31290000,-0.28,0.014,-0.0055,0.96,0.016,0.00047,0.76,0.056,-0.038,-0.72,-0.0012,-0.0058,3.4e-05,0.024,-0.016,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.022,0.026,0.026,0.008,0.24,0.24,0.035,3.2e-07,3.2e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -31390000,-0.28,0.014,-0.0052,0.96,0.011,0.0042,0.76,0.05,-0.036,-0.65,-0.0012,-0.0058,3.4e-05,0.024,-0.016,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.022,0.025,0.025,0.0079,0.24,0.24,0.035,3.1e-07,3.1e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -31490000,-0.28,0.014,-0.0049,0.96,0.01,0.0072,0.76,0.051,-0.035,-0.58,-0.0012,-0.0058,3.4e-05,0.024,-0.016,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.022,0.026,0.026,0.008,0.25,0.25,0.035,3.2e-07,3.1e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -31590000,-0.28,0.015,-0.0048,0.96,0.011,0.0092,0.76,0.05,-0.031,-0.51,-0.0012,-0.0058,3.4e-05,0.024,-0.016,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.022,0.025,0.025,0.0079,0.25,0.25,0.035,3.1e-07,3.1e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -31690000,-0.28,0.015,-0.0047,0.96,0.013,0.011,0.76,0.051,-0.03,-0.44,-0.0012,-0.0058,3.4e-05,0.024,-0.017,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.022,0.026,0.026,0.008,0.26,0.26,0.035,3.1e-07,3.1e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -31790000,-0.28,0.016,-0.0049,0.96,0.0088,0.017,0.76,0.049,-0.02,-0.36,-0.0012,-0.0058,3.4e-05,0.024,-0.017,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.022,0.025,0.025,0.008,0.26,0.26,0.035,3.1e-07,3e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -31890000,-0.28,0.015,-0.0047,0.96,0.0053,0.019,0.76,0.05,-0.018,-0.3,-0.0012,-0.0058,3.4e-05,0.024,-0.017,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.022,0.026,0.026,0.008,0.27,0.27,0.035,3.1e-07,3e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -31990000,-0.28,0.015,-0.005,0.96,0.0027,0.02,0.75,0.049,-0.014,-0.23,-0.0012,-0.0058,3.3e-05,0.024,-0.017,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.022,0.025,0.025,0.0079,0.27,0.27,0.035,3e-07,3e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -32090000,-0.28,0.015,-0.0054,0.96,0.0029,0.024,0.76,0.049,-0.012,-0.16,-0.0012,-0.0058,3.3e-05,0.024,-0.017,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.022,0.026,0.026,0.008,0.28,0.28,0.035,3e-07,3e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -32190000,-0.28,0.015,-0.0056,0.96,0.00043,0.03,0.76,0.046,-0.0031,-0.092,-0.0012,-0.0058,3.3e-05,0.024,-0.017,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.022,0.025,0.025,0.0079,0.28,0.28,0.035,3e-07,3e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -32290000,-0.28,0.015,-0.0055,0.96,-0.0013,0.033,0.75,0.046,0.0001,-0.024,-0.0012,-0.0058,3.3e-05,0.024,-0.017,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.022,0.026,0.026,0.008,0.29,0.29,0.035,3e-07,3e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -32390000,-0.28,0.015,-0.0056,0.96,-0.0041,0.034,0.75,0.044,0.0027,0.051,-0.0012,-0.0058,3.3e-05,0.024,-0.017,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00035,0.00034,0.022,0.025,0.025,0.008,0.29,0.29,0.035,3e-07,2.9e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -32490000,-0.28,0.013,-0.0086,0.96,-0.042,0.092,-0.12,0.041,0.011,0.054,-0.0012,-0.0058,3.3e-05,0.025,-0.017,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00034,0.022,0.028,0.028,0.0078,0.3,0.3,0.035,3e-07,2.9e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -32590000,-0.28,0.013,-0.0086,0.96,-0.036,0.091,-0.12,0.047,0.003,0.035,-0.0012,-0.0058,3.4e-05,0.025,-0.017,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00034,0.022,0.028,0.028,0.0075,0.3,0.3,0.035,2.9e-07,2.9e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -32690000,-0.28,0.013,-0.0086,0.96,-0.032,0.098,-0.12,0.043,0.012,0.02,-0.0012,-0.0058,3.4e-05,0.025,-0.017,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00034,0.00034,0.022,0.031,0.031,0.0074,0.31,0.31,0.035,2.9e-07,2.9e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -32790000,-0.28,0.013,-0.0083,0.96,-0.026,0.094,-0.12,0.048,0.0036,0.0046,-0.0012,-0.0058,3.4e-05,0.025,-0.017,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00033,0.00033,0.022,0.031,0.031,0.0071,0.31,0.31,0.035,2.9e-07,2.8e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -32890000,-0.28,0.013,-0.0082,0.96,-0.026,0.1,-0.13,0.045,0.013,-0.011,-0.0012,-0.0058,3.4e-05,0.025,-0.017,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00033,0.00033,0.022,0.035,0.035,0.007,0.32,0.32,0.035,2.9e-07,2.9e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -32990000,-0.28,0.013,-0.0079,0.96,-0.02,0.091,-0.12,0.049,-0.00055,-0.024,-0.0012,-0.0058,3.5e-05,0.025,-0.017,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00031,0.00031,0.022,0.035,0.035,0.0067,0.31,0.31,0.035,2.9e-07,2.8e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -33090000,-0.28,0.013,-0.0078,0.96,-0.016,0.096,-0.12,0.047,0.0088,-0.031,-0.0012,-0.0058,3.5e-05,0.025,-0.017,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00031,0.00031,0.022,0.04,0.04,0.0066,0.33,0.33,0.035,2.9e-07,2.8e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -33190000,-0.28,0.013,-0.0074,0.96,-0.0097,0.086,-0.12,0.05,-0.0068,-0.037,-0.0013,-0.0058,3.5e-05,0.024,-0.018,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00029,0.00029,0.022,0.039,0.04,0.0064,0.32,0.32,0.035,2.8e-07,2.8e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -33290000,-0.28,0.013,-0.0074,0.96,-0.0064,0.088,-0.12,0.049,0.0019,-0.045,-0.0013,-0.0058,3.5e-05,0.024,-0.018,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00029,0.00029,0.022,0.045,0.046,0.0063,0.34,0.34,0.034,2.8e-07,2.8e-07,6.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -33390000,-0.28,0.013,-0.0072,0.96,-0.0012,0.078,-0.12,0.05,-0.0052,-0.053,-0.0013,-0.0057,3.6e-05,0.024,-0.019,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00027,0.00026,0.022,0.044,0.045,0.0062,0.33,0.33,0.034,2.8e-07,2.8e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 -33490000,-0.28,0.013,-0.0072,0.96,0.0032,0.081,-0.12,0.05,0.0028,-0.062,-0.0013,-0.0057,3.6e-05,0.024,-0.019,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00027,0.00026,0.022,0.051,0.052,0.0061,0.35,0.35,0.034,2.8e-07,2.8e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 -33590000,-0.28,0.013,-0.0068,0.96,0.006,0.07,-0.11,0.05,-0.0075,-0.069,-0.0013,-0.0057,4.1e-05,0.022,-0.021,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00024,0.00024,0.018,0.049,0.05,0.006,0.34,0.34,0.034,2.8e-07,2.7e-07,6.2e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -33690000,-0.28,0.013,-0.0068,0.96,0.01,0.072,-0.11,0.05,-0.00042,-0.077,-0.0013,-0.0057,4.1e-05,0.022,-0.021,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00024,0.00024,0.018,0.057,0.058,0.0059,0.36,0.36,0.034,2.8e-07,2.7e-07,6.2e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -33790000,-0.28,0.013,-0.0065,0.96,0.013,0.061,-0.11,0.049,-0.0092,-0.083,-0.0013,-0.0057,4.1e-05,0.019,-0.024,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00022,0.00021,0.018,0.053,0.054,0.0059,0.35,0.35,0.034,2.8e-07,2.7e-07,6.2e-06,0.026,0.027,0.0005,0,0,0,0,0,0,0,0 -33890000,-0.28,0.013,-0.0066,0.96,0.017,0.06,-0.11,0.051,-0.0032,-0.089,-0.0013,-0.0057,4.1e-05,0.019,-0.024,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00022,0.00021,0.018,0.061,0.062,0.0058,0.37,0.37,0.033,2.8e-07,2.7e-07,6.2e-06,0.026,0.027,0.0005,0,0,0,0,0,0,0,0 -33990000,-0.28,0.013,-0.0064,0.96,0.019,0.052,-0.1,0.051,-0.0059,-0.092,-0.0013,-0.0057,4.2e-05,0.018,-0.025,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.0002,0.00019,0.018,0.056,0.056,0.0058,0.36,0.36,0.033,2.8e-07,2.7e-07,6.2e-06,0.026,0.026,0.0005,0,0,0,0,0,0,0,0 -34090000,-0.28,0.013,-0.0064,0.96,0.023,0.054,-0.1,0.053,-0.00048,-0.096,-0.0013,-0.0057,4.2e-05,0.018,-0.025,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.0002,0.00019,0.018,0.064,0.064,0.0058,0.37,0.37,0.033,2.8e-07,2.7e-07,6.2e-06,0.026,0.026,0.0005,0,0,0,0,0,0,0,0 -34190000,-0.28,0.014,-0.0063,0.96,0.023,0.046,-0.098,0.051,-0.0045,-0.098,-0.0013,-0.0057,4.2e-05,0.015,-0.026,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00018,0.00018,0.018,0.057,0.058,0.0058,0.37,0.37,0.033,2.8e-07,2.7e-07,6.2e-06,0.025,0.025,0.0005,0,0,0,0,0,0,0,0 -34290000,-0.28,0.014,-0.0062,0.96,0.023,0.046,-0.096,0.053,6.1e-05,-0.1,-0.0013,-0.0057,4.2e-05,0.015,-0.026,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00018,0.00018,0.018,0.065,0.065,0.0058,0.38,0.38,0.033,2.8e-07,2.7e-07,6.2e-06,0.025,0.025,0.0005,0,0,0,0,0,0,0,0 -34390000,-0.28,0.014,-0.0061,0.96,0.022,0.038,-0.091,0.05,-0.003,-0.11,-0.0013,-0.0057,4.2e-05,0.013,-0.027,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00017,0.00016,0.018,0.058,0.058,0.0059,0.38,0.38,0.033,2.8e-07,2.7e-07,6.2e-06,0.024,0.024,0.0005,0,0,0,0,0,0,0,0 -34490000,-0.28,0.014,-0.0062,0.96,0.024,0.038,-0.089,0.053,0.00081,-0.11,-0.0013,-0.0057,4.2e-05,0.013,-0.027,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00017,0.00016,0.018,0.064,0.065,0.0059,0.39,0.39,0.032,2.8e-07,2.8e-07,6.2e-06,0.024,0.024,0.0005,0,0,0,0,0,0,0,0 -34590000,-0.28,0.013,-0.0061,0.96,0.021,0.029,0.71,0.05,-0.0034,-0.08,-0.0013,-0.0057,4.2e-05,0.011,-0.028,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00016,0.00015,0.018,0.055,0.055,0.0059,0.39,0.39,0.032,2.8e-07,2.8e-07,6.2e-06,0.023,0.023,0.0005,0,0,0,0,0,0,0,0 -34690000,-0.28,0.013,-0.0061,0.96,0.024,0.027,1.7,0.052,-0.0006,0.038,-0.0013,-0.0057,4.2e-05,0.011,-0.028,-0.11,-0.017,-0.0037,0.57,0,0,0,0,0,0.00016,0.00015,0.018,0.058,0.058,0.006,0.4,0.4,0.032,2.8e-07,2.8e-07,6.2e-06,0.023,0.023,0.0005,0,0,0,0,0,0,0,0 -34790000,-0.28,0.013,-0.0061,0.96,0.024,0.019,2.7,0.05,-0.0033,0.21,-0.0013,-0.0057,4.2e-05,0.011,-0.031,-0.1,-0.017,-0.0037,0.57,0,0,0,0,0,0.00015,0.00015,0.018,0.05,0.05,0.0061,0.4,0.4,0.032,2.8e-07,2.8e-07,6.2e-06,0.022,0.022,0.0005,0,0,0,0,0,0,0,0 -34890000,-0.28,0.013,-0.0061,0.96,0.027,0.017,3.6,0.052,-0.0015,0.5,-0.0013,-0.0057,4.2e-05,0.012,-0.031,-0.1,-0.017,-0.0037,0.57,0,0,0,0,0,0.00015,0.00015,0.018,0.054,0.054,0.0061,0.41,0.41,0.032,2.8e-07,2.8e-07,6.2e-06,0.022,0.022,0.0005,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.9e-06,-0.00042,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0.01,0.00018,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +90000,1,-0.011,-0.01,0.00033,-0.001,-0.0031,-0.024,-3.8e-05,-0.00013,-0.0021,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0.01,0.00046,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +190000,1,-0.012,-0.011,0.00044,-0.0023,-0.003,-0.037,-0.00017,-0.00043,-0.017,4.7e-10,-5e-10,-2.1e-11,0,0,-1.1e-06,0,0,0,0,0,0,0,0,0.011,0.011,0.00094,25,25,10,1e+02,1e+02,1,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +290000,1,-0.012,-0.011,0.00044,-0.0033,-0.0044,-0.046,-0.00024,-0.00025,-0.018,3.8e-09,-5.9e-09,-2.1e-10,0,0,-1e-05,0,0,0,0,0,0,0,0,0.012,0.012,0.00077,25,25,9.6,0.37,0.37,0.41,0.01,0.01,0.0052,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +390000,1,-0.012,-0.011,0.00049,-0.0025,-0.0059,-0.063,-0.00056,-0.00071,-0.013,-7.1e-09,-5.8e-09,1.5e-11,0,0,2.2e-06,0,0,0,0,0,0,0,0,0.012,0.012,0.0012,25,25,8.1,0.97,0.97,0.32,0.01,0.01,0.0052,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +490000,1,-0.012,-0.012,0.00055,-0.0007,-0.0062,-0.069,-0.00015,-0.00046,-0.011,-1.2e-06,7.4e-07,4.1e-08,0,0,-1e-06,0,0,0,0,0,0,0,0,0.013,0.013,0.00073,7.8,7.8,5.9,0.34,0.34,0.31,0.01,0.01,0.0024,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +590000,1,-0.012,-0.012,0.00057,-0.002,-0.009,-0.12,-0.00028,-0.0012,-0.029,-1.3e-06,7.7e-07,4.5e-08,0,0,7.8e-05,0,0,0,0,0,0,0,0,0.015,0.015,0.001,7.9,7.9,4.2,0.67,0.67,0.32,0.01,0.01,0.0024,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +690000,1,-0.012,-0.012,0.0006,5.4e-05,-0.0088,-0.05,-8e-05,-0.00078,-0.0088,-5.6e-06,1.6e-06,1.6e-07,0,0,-0.00016,0,0,0,0,0,0,0,0,0.016,0.016,0.00063,2.7,2.7,2.8,0.26,0.26,0.29,0.01,0.01,0.0012,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +790000,1,-0.012,-0.012,0.0006,0.0022,-0.01,-0.054,-2.3e-05,-0.0017,-0.011,-5.4e-06,1.6e-06,1.5e-07,0,0,-0.0002,0,0,0,0,0,0,0,0,0.018,0.018,0.0008,2.8,2.8,1.9,0.42,0.42,0.27,0.01,0.01,0.0012,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +890000,1,-0.012,-0.013,0.00061,0.0031,-0.0084,-0.093,0.00015,-0.0011,-0.031,-2.1e-05,1e-06,4.9e-07,0,0,-8.1e-05,0,0,0,0,0,0,0,0,0.019,0.019,0.00054,1.3,1.3,1.3,0.2,0.2,0.25,0.0099,0.0099,0.00068,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +990000,1,-0.012,-0.013,0.00058,0.006,-0.0097,-0.12,0.00062,-0.002,-0.046,-2.2e-05,1e-06,5e-07,0,0,-2.6e-05,0,0,0,0,0,0,0,0,0.021,0.021,0.00066,1.5,1.5,0.95,0.3,0.3,0.23,0.0099,0.0099,0.00068,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +1090000,1,-0.012,-0.013,0.00054,0.011,-0.013,-0.13,0.00077,-0.0014,-0.062,-6e-05,-1.5e-05,9.9e-07,0,0,1.1e-05,0,0,0,0,0,0,0,0,0.023,0.023,0.00047,0.93,0.93,0.69,0.17,0.17,0.2,0.0098,0.0098,0.00043,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +1190000,1,-0.012,-0.013,0.00047,0.015,-0.018,-0.11,0.0021,-0.003,-0.047,-5.8e-05,-1.3e-05,9.7e-07,0,0,-0.00056,0,0,0,0,0,0,0,0,0.025,0.025,0.00056,1.1,1.1,0.54,0.24,0.24,0.19,0.0098,0.0098,0.00043,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +1290000,1,-0.012,-0.014,0.00042,0.019,-0.018,-0.11,0.0019,-0.0024,-0.048,-0.00017,-9.7e-05,1.5e-06,0,0,-0.00083,0,0,0,0,0,0,0,0,0.026,0.026,0.00042,0.89,0.89,0.42,0.15,0.15,0.18,0.0095,0.0095,0.00029,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +1390000,1,-0.012,-0.014,0.00038,0.026,-0.023,-0.097,0.0043,-0.0044,-0.038,-0.00016,-9.2e-05,1.5e-06,0,0,-0.0015,0,0,0,0,0,0,0,0,0.028,0.028,0.00049,1.2,1.2,0.33,0.21,0.21,0.16,0.0095,0.0095,0.00029,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +1490000,1,-0.012,-0.014,0.00038,0.024,-0.02,-0.12,0.0034,-0.0032,-0.053,-0.00039,-0.00033,1.3e-06,0,0,-0.0013,0,0,0,0,0,0,0,0,0.027,0.027,0.00038,0.96,0.96,0.27,0.14,0.14,0.15,0.0088,0.0088,0.0002,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +1590000,1,-0.012,-0.014,0.00039,0.031,-0.024,-0.13,0.0061,-0.0055,-0.063,-0.00039,-0.00033,1.3e-06,0,0,-0.0015,0,0,0,0,0,0,0,0,0.03,0.03,0.00043,1.3,1.3,0.23,0.2,0.2,0.14,0.0088,0.0088,0.0002,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +1690000,1,-0.012,-0.014,0.00044,0.028,-0.019,-0.13,0.0043,-0.0037,-0.068,-0.00073,-0.00074,-7.7e-08,0,0,-0.0019,0,0,0,0,0,0,0,0,0.026,0.026,0.00034,1,1,0.19,0.14,0.14,0.13,0.0078,0.0078,0.00015,0.04,0.04,0.039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +1790000,1,-0.012,-0.014,0.0004,0.035,-0.024,-0.13,0.0076,-0.0059,-0.067,-0.00073,-0.00073,-3.3e-08,0,0,-0.0029,0,0,0,0,0,0,0,0,0.028,0.028,0.00039,1.3,1.3,0.17,0.2,0.2,0.12,0.0078,0.0078,0.00015,0.04,0.04,0.039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +1890000,1,-0.012,-0.014,0.00039,0.043,-0.025,-0.14,0.011,-0.0083,-0.075,-0.00072,-0.00072,-7e-09,0,0,-0.0033,0,0,0,0,0,0,0,0,0.031,0.031,0.00043,1.7,1.7,0.15,0.31,0.31,0.12,0.0078,0.0078,0.00015,0.04,0.04,0.039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +1990000,1,-0.011,-0.014,0.0004,0.035,-0.018,-0.14,0.0082,-0.0053,-0.074,-0.0011,-0.0013,-3.1e-06,0,0,-0.0047,0,0,0,0,0,0,0,0,0.025,0.025,0.00035,1.3,1.3,0.13,0.2,0.2,0.11,0.0067,0.0067,0.00011,0.04,0.04,0.039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +2090000,1,-0.011,-0.014,0.00043,0.042,-0.02,-0.14,0.012,-0.0073,-0.071,-0.0011,-0.0012,-3e-06,0,0,-0.0066,0,0,0,0,0,0,0,0,0.027,0.027,0.00039,1.7,1.7,0.12,0.31,0.31,0.11,0.0067,0.0067,0.00011,0.04,0.04,0.039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +2190000,1,-0.011,-0.014,0.00039,0.033,-0.013,-0.14,0.0081,-0.0043,-0.077,-0.0014,-0.0018,-7.8e-06,0,0,-0.0076,0,0,0,0,0,0,0,0,0.02,0.02,0.00031,1.2,1.2,0.11,0.2,0.2,0.11,0.0055,0.0055,9e-05,0.04,0.04,0.038,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +2290000,1,-0.011,-0.014,0.00038,0.038,-0.014,-0.14,0.012,-0.0057,-0.075,-0.0014,-0.0018,-7.6e-06,0,0,-0.0099,0,0,0,0,0,0,0,0,0.022,0.022,0.00034,1.5,1.5,0.11,0.3,0.3,0.1,0.0055,0.0055,9e-05,0.04,0.04,0.038,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +2390000,1,-0.011,-0.013,0.0004,0.029,-0.0098,-0.14,0.0075,-0.0033,-0.072,-0.0017,-0.0023,-1.3e-05,0,0,-0.013,0,0,0,0,0,0,0,0,0.017,0.017,0.00028,1,1,0.1,0.19,0.19,0.098,0.0046,0.0046,7.1e-05,0.04,0.04,0.037,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +2490000,1,-0.011,-0.014,0.00047,0.033,-0.0088,-0.14,0.011,-0.0042,-0.079,-0.0017,-0.0023,-1.3e-05,0,0,-0.014,0,0,0,0,0,0,0,0,0.018,0.018,0.00031,1.3,1.3,0.1,0.28,0.28,0.097,0.0046,0.0046,7.1e-05,0.04,0.04,0.037,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +2590000,1,-0.01,-0.013,0.00039,0.023,-0.0059,-0.15,0.0066,-0.0023,-0.084,-0.0018,-0.0027,-1.9e-05,0,0,-0.015,0,0,0,0,0,0,0,0,0.014,0.014,0.00026,0.89,0.89,0.099,0.18,0.18,0.094,0.0038,0.0038,5.8e-05,0.04,0.04,0.036,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +2690000,1,-0.01,-0.013,0.00043,0.027,-0.0051,-0.15,0.0091,-0.0029,-0.084,-0.0018,-0.0027,-1.8e-05,0,0,-0.018,0,0,0,0,0,0,0,0,0.015,0.015,0.00028,1.1,1.1,0.097,0.25,0.25,0.091,0.0038,0.0038,5.8e-05,0.04,0.04,0.036,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +2790000,1,-0.01,-0.013,0.00037,0.022,-0.0029,-0.14,0.0059,-0.0016,-0.081,-0.0019,-0.003,-2.3e-05,0,0,-0.022,0,0,0,0,0,0,0,0,0.011,0.011,0.00024,0.77,0.77,0.095,0.16,0.16,0.089,0.0032,0.0032,4.8e-05,0.04,0.04,0.035,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +2890000,1,-0.01,-0.013,0.0003,0.026,-0.0046,-0.14,0.0082,-0.002,-0.081,-0.0019,-0.003,-2.3e-05,0,0,-0.026,0,0,0,0,0,0,0,0,0.013,0.013,0.00026,0.95,0.95,0.096,0.23,0.23,0.089,0.0032,0.0032,4.8e-05,0.04,0.04,0.034,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +2990000,1,-0.01,-0.013,0.00031,0.02,-0.0035,-0.15,0.0054,-0.0012,-0.086,-0.002,-0.0033,-2.8e-05,0,0,-0.028,0,0,0,0,0,0,0,0,0.0099,0.0099,0.00022,0.67,0.67,0.095,0.15,0.15,0.088,0.0027,0.0027,4e-05,0.04,0.04,0.033,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +3090000,1,-0.01,-0.013,0.00052,0.025,-0.0063,-0.15,0.0077,-0.0018,-0.087,-0.002,-0.0033,-2.8e-05,0,0,-0.031,0,0,0,0,0,0,0,0,0.011,0.011,0.00024,0.83,0.83,0.095,0.22,0.22,0.086,0.0027,0.0027,4e-05,0.04,0.04,0.032,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +3190000,1,-0.01,-0.013,0.00056,0.02,-0.0061,-0.15,0.0051,-0.0013,-0.097,-0.002,-0.0036,-3.2e-05,0,0,-0.033,0,0,0,0,0,0,0,0,0.0088,0.0088,0.00021,0.59,0.59,0.096,0.14,0.14,0.087,0.0023,0.0023,3.4e-05,0.04,0.04,0.031,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +3290000,1,-0.01,-0.013,0.00059,0.023,-0.0062,-0.15,0.0073,-0.002,-0.11,-0.002,-0.0036,-3.2e-05,0,0,-0.035,0,0,0,0,0,0,0,0,0.0096,0.0096,0.00022,0.73,0.73,0.095,0.2,0.2,0.086,0.0023,0.0023,3.4e-05,0.04,0.04,0.03,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +3390000,1,-0.0098,-0.013,0.0006,0.019,-0.0032,-0.15,0.0049,-0.0013,-0.1,-0.0021,-0.0038,-3.5e-05,0,0,-0.04,0,0,0,0,0,0,0,0,0.0078,0.0078,0.00019,0.53,0.53,0.095,0.14,0.14,0.085,0.002,0.002,2.9e-05,0.04,0.04,0.029,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +3490000,1,-0.0097,-0.013,0.00058,0.025,-0.0018,-0.15,0.0072,-0.0016,-0.1,-0.0021,-0.0038,-3.5e-05,0,0,-0.044,0,0,0,0,0,0,0,0,0.0086,0.0086,0.00021,0.66,0.66,0.095,0.19,0.19,0.086,0.002,0.002,2.9e-05,0.04,0.04,0.027,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +3590000,1,-0.0095,-0.012,0.00054,0.021,-0.0014,-0.15,0.0051,-0.00091,-0.11,-0.0022,-0.004,-3.9e-05,0,0,-0.047,0,0,0,0,0,0,0,0,0.007,0.0071,0.00018,0.49,0.49,0.094,0.13,0.13,0.086,0.0017,0.0017,2.5e-05,0.04,0.04,0.026,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +3690000,1,-0.0095,-0.013,0.00052,0.024,-0.00063,-0.15,0.0074,-0.0011,-0.11,-0.0022,-0.004,-3.9e-05,0,0,-0.052,0,0,0,0,0,0,0,0,0.0077,0.0077,0.00019,0.6,0.6,0.093,0.18,0.18,0.085,0.0017,0.0017,2.5e-05,0.04,0.04,0.025,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +3790000,1,-0.0094,-0.012,0.00055,0.019,0.0038,-0.15,0.0051,-0.00046,-0.11,-0.0022,-0.0043,-4.3e-05,0,0,-0.055,0,0,0,0,0,0,0,0,0.0064,0.0064,0.00017,0.45,0.45,0.093,0.12,0.12,0.086,0.0014,0.0014,2.2e-05,0.04,0.04,0.024,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +3890000,1,-0.0094,-0.013,0.00063,0.021,0.005,-0.14,0.0072,-1.9e-05,-0.11,-0.0022,-0.0042,-4.3e-05,0,0,-0.059,0,0,0,0,0,0,0,0,0.0069,0.0069,0.00018,0.55,0.55,0.091,0.17,0.17,0.086,0.0014,0.0014,2.2e-05,0.04,0.04,0.022,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +3990000,1,-0.0094,-0.013,0.0007,0.026,0.0048,-0.14,0.0096,0.00041,-0.11,-0.0022,-0.0042,-4.3e-05,0,0,-0.064,0,0,0,0,0,0,0,0,0.0075,0.0075,0.00019,0.66,0.66,0.089,0.23,0.23,0.085,0.0014,0.0014,2.2e-05,0.04,0.04,0.021,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +4090000,1,-0.0093,-0.012,0.00076,0.022,0.0042,-0.12,0.0071,0.0006,-0.098,-0.0022,-0.0044,-4.8e-05,0,0,-0.072,0,0,0,0,0,0,0,0,0.0062,0.0062,0.00017,0.5,0.5,0.087,0.16,0.16,0.085,0.0012,0.0012,1.9e-05,0.04,0.04,0.02,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +4190000,1,-0.0094,-0.012,0.00073,0.024,0.0039,-0.12,0.0094,0.001,-0.1,-0.0022,-0.0044,-4.8e-05,0,0,-0.074,0,0,0,0,0,0,0,0,0.0068,0.0068,0.00018,0.61,0.61,0.086,0.21,0.21,0.086,0.0012,0.0012,1.9e-05,0.04,0.04,0.019,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +4290000,1,-0.0095,-0.012,0.00074,0.021,0.0037,-0.12,0.0068,0.00083,-0.11,-0.0021,-0.0046,-5.3e-05,0,0,-0.077,0,0,0,0,0,0,0,0,0.0056,0.0056,0.00016,0.47,0.47,0.084,0.15,0.15,0.085,0.00097,0.00097,1.7e-05,0.04,0.04,0.017,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +4390000,1,-0.0094,-0.012,0.0007,0.025,0.0023,-0.11,0.0091,0.0011,-0.094,-0.0021,-0.0046,-5.2e-05,0,0,-0.083,0,0,0,0,0,0,0,0,0.006,0.006,0.00017,0.56,0.56,0.081,0.2,0.2,0.084,0.00097,0.00097,1.7e-05,0.04,0.04,0.016,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +4490000,1,-0.0094,-0.012,0.00076,0.021,0.004,-0.11,0.0067,0.00086,-0.095,-0.0021,-0.0048,-5.7e-05,0,0,-0.086,0,0,0,0,0,0,0,0,0.005,0.005,0.00016,0.43,0.43,0.08,0.14,0.14,0.085,0.0008,0.0008,1.5e-05,0.04,0.04,0.015,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +4590000,1,-0.0094,-0.012,0.00083,0.023,0.0029,-0.11,0.0089,0.0012,-0.098,-0.0021,-0.0048,-5.7e-05,0,0,-0.088,0,0,0,0,0,0,0,0,0.0054,0.0054,0.00016,0.52,0.52,0.077,0.19,0.19,0.084,0.0008,0.0008,1.5e-05,0.04,0.04,0.014,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +4690000,1,-0.0094,-0.012,0.00076,0.017,0.0031,-0.1,0.0064,0.00089,-0.09,-0.0021,-0.005,-6.1e-05,0,0,-0.093,0,0,0,0,0,0,0,0,0.0044,0.0044,0.00015,0.4,0.4,0.074,0.14,0.14,0.083,0.00065,0.00065,1.3e-05,0.04,0.04,0.013,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +4790000,1,-0.0093,-0.012,0.00086,0.015,0.0053,-0.099,0.008,0.0014,-0.092,-0.0021,-0.005,-6.1e-05,0,0,-0.095,0,0,0,0,0,0,0,0,0.0047,0.0047,0.00016,0.47,0.47,0.073,0.18,0.18,0.084,0.00065,0.00065,1.3e-05,0.04,0.04,0.012,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +4890000,1,-0.0092,-0.012,0.0009,0.01,0.0028,-0.093,0.0053,0.001,-0.088,-0.0021,-0.0051,-6.5e-05,0,0,-0.099,0,0,0,0,0,0,0,0,0.0039,0.0039,0.00014,0.36,0.36,0.07,0.13,0.13,0.083,0.00053,0.00053,1.2e-05,0.04,0.04,0.011,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +4990000,1,-0.0092,-0.012,0.00089,0.013,0.0035,-0.085,0.0065,0.0014,-0.083,-0.0021,-0.0051,-6.5e-05,0,0,-0.1,0,0,0,0,0,0,0,0,0.0042,0.0042,0.00015,0.43,0.43,0.067,0.17,0.17,0.082,0.00053,0.00053,1.2e-05,0.04,0.04,0.011,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +5090000,1,-0.0091,-0.011,0.00096,0.01,0.0038,-0.082,0.0045,0.001,-0.082,-0.002,-0.0052,-6.8e-05,0,0,-0.1,0,0,0,0,0,0,0,0,0.0034,0.0034,0.00014,0.33,0.33,0.065,0.12,0.12,0.082,0.00043,0.00043,1e-05,0.04,0.04,0.0098,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +5190000,1,-0.0089,-0.012,0.001,0.0099,0.0074,-0.08,0.0055,0.0015,-0.079,-0.002,-0.0052,-6.8e-05,0,0,-0.11,0,0,0,0,0,0,0,0,0.0037,0.0037,0.00014,0.39,0.39,0.063,0.16,0.16,0.081,0.00043,0.00043,1e-05,0.04,0.04,0.0091,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +5290000,1,-0.0089,-0.011,0.0011,0.0082,0.0074,-0.068,0.0038,0.0014,-0.072,-0.002,-0.0053,-7e-05,0,0,-0.11,0,0,0,0,0,0,0,0,0.003,0.003,0.00013,0.3,0.3,0.06,0.12,0.12,0.08,0.00035,0.00035,9.3e-06,0.04,0.04,0.0084,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +5390000,1,-0.0088,-0.011,0.0011,0.0077,0.011,-0.065,0.0046,0.0022,-0.067,-0.002,-0.0053,-7e-05,0,0,-0.11,0,0,0,0,0,0,0,0,0.0032,0.0032,0.00014,0.36,0.36,0.057,0.16,0.16,0.079,0.00035,0.00035,9.3e-06,0.04,0.04,0.0078,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +5490000,1,-0.0088,-0.011,0.0011,0.0072,0.012,-0.06,0.0031,0.0021,-0.065,-0.002,-0.0054,-7.2e-05,0,0,-0.11,0,0,0,0,0,0,0,0,0.0027,0.0027,0.00013,0.28,0.28,0.056,0.11,0.11,0.079,0.00028,0.00028,8.4e-06,0.04,0.04,0.0073,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +5590000,1,-0.0088,-0.012,0.00099,0.0083,0.016,-0.053,0.004,0.0035,-0.058,-0.002,-0.0054,-7.2e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.0028,0.0028,0.00013,0.33,0.33,0.053,0.15,0.15,0.078,0.00028,0.00028,8.4e-06,0.04,0.04,0.0067,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +5690000,1,-0.0089,-0.011,0.00089,0.0077,0.016,-0.052,0.0028,0.003,-0.055,-0.0019,-0.0054,-7.5e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.0024,0.0024,0.00012,0.25,0.25,0.051,0.11,0.11,0.076,0.00023,0.00023,7.6e-06,0.04,0.04,0.0063,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +5790000,1,-0.0088,-0.011,0.00085,0.0089,0.018,-0.049,0.0036,0.0047,-0.053,-0.0019,-0.0054,-7.5e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.0025,0.0025,0.00013,0.3,0.3,0.05,0.14,0.14,0.077,0.00023,0.00023,7.6e-06,0.04,0.04,0.0059,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +5890000,1,-0.0088,-0.011,0.00088,0.0095,0.015,-0.048,0.0027,0.0038,-0.056,-0.0019,-0.0055,-7.7e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.0021,0.0021,0.00012,0.23,0.23,0.047,0.1,0.1,0.075,0.00018,0.00018,6.9e-06,0.04,0.04,0.0054,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +5990000,1,-0.0088,-0.012,0.00086,0.011,0.017,-0.041,0.0038,0.0054,-0.05,-0.0019,-0.0055,-7.7e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.0022,0.0022,0.00012,0.27,0.27,0.045,0.13,0.13,0.074,0.00018,0.00018,6.9e-06,0.04,0.04,0.005,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +6090000,1,-0.0088,-0.011,0.00067,0.011,0.018,-0.039,0.0049,0.0072,-0.047,-0.0019,-0.0055,-7.7e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.0023,0.0023,0.00013,0.31,0.31,0.044,0.17,0.17,0.074,0.00018,0.00018,6.9e-06,0.04,0.04,0.0047,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +6190000,1,-0.0089,-0.011,0.00068,0.0087,0.017,-0.038,0.0038,0.0057,-0.047,-0.0018,-0.0055,-8e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.002,0.002,0.00012,0.24,0.24,0.042,0.13,0.13,0.073,0.00015,0.00015,6.3e-06,0.04,0.04,0.0044,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +6290000,1,-0.0089,-0.011,0.00071,0.008,0.019,-0.041,0.0046,0.0075,-0.053,-0.0018,-0.0055,-8e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.0021,0.0021,0.00012,0.28,0.28,0.04,0.16,0.16,0.072,0.00015,0.00015,6.3e-06,0.04,0.04,0.0041,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +6390000,1,-0.0089,-0.011,0.00072,0.0082,0.016,-0.042,0.0034,0.006,-0.056,-0.0017,-0.0056,-8.2e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.0017,0.0017,0.00011,0.22,0.22,0.039,0.12,0.12,0.072,0.00012,0.00012,5.8e-06,0.04,0.04,0.0039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +6490000,1,-0.0089,-0.011,0.00062,0.0057,0.016,-0.039,0.0041,0.0076,-0.053,-0.0017,-0.0056,-8.2e-05,0,0,-0.13,0,0,0,0,0,0,0,0,0.0018,0.0018,0.00012,0.25,0.25,0.038,0.15,0.15,0.07,0.00012,0.00012,5.8e-06,0.04,0.04,0.0036,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +6590000,1,-0.009,-0.011,0.00055,0.0039,0.015,-0.042,0.0029,0.0058,-0.056,-0.0017,-0.0056,-8.5e-05,0,0,-0.13,0,0,0,0,0,0,0,0,0.0016,0.0016,0.00011,0.2,0.2,0.036,0.12,0.12,0.069,9.8e-05,9.8e-05,5.3e-06,0.04,0.04,0.0034,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +6690000,1,-0.0089,-0.011,0.0005,0.0022,0.018,-0.044,0.0032,0.0075,-0.057,-0.0017,-0.0056,-8.5e-05,0,0,-0.13,0,0,0,0,0,0,0,0,0.0016,0.0016,0.00011,0.23,0.23,0.035,0.14,0.14,0.068,9.8e-05,9.8e-05,5.3e-06,0.04,0.04,0.0031,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +6790000,1,-0.009,-0.011,0.00047,0.003,0.015,-0.042,0.0021,0.0059,-0.058,-0.0016,-0.0056,-8.6e-05,0,0,-0.13,0,0,0,0,0,0,0,0,0.0014,0.0014,0.00011,0.18,0.18,0.034,0.11,0.11,0.068,8e-05,8.1e-05,4.9e-06,0.04,0.04,0.003,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +6890000,1,-0.0088,-0.011,0.00039,0.0023,0.015,-0.039,0.0024,0.0074,-0.055,-0.0016,-0.0056,-8.6e-05,0,0,-0.13,0,0,0,0,0,0,0,0,0.0015,0.0015,0.00011,0.21,0.21,0.032,0.14,0.14,0.067,8e-05,8.1e-05,4.9e-06,0.04,0.04,0.0028,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 +6990000,-0.29,0.024,-0.0065,0.96,-0.0055,0.0091,-0.037,0.0023,0.0053,-0.055,-0.0016,-0.0056,-8.7e-05,0,0,-0.13,-0.092,-0.02,0.51,0.069,-0.028,-0.058,0,0,0.0012,0.0012,0.072,0.16,0.16,0.031,0.11,0.11,0.066,6.6e-05,6.6e-05,4.6e-06,0.04,0.04,0.0026,0.0014,0.00048,0.0014,0.0014,0.0012,0.0014,1,1 +7090000,-0.28,0.025,-0.0064,0.96,-0.026,-0.00083,-0.037,0.0022,0.0052,-0.056,-0.0015,-0.0055,-8.7e-05,0,0,-0.13,-0.099,-0.021,0.51,0.075,-0.029,-0.065,0,0,0.0012,0.0012,0.058,0.16,0.16,0.03,0.13,0.13,0.066,6.6e-05,6.6e-05,4.6e-06,0.04,0.04,0.0024,0.0013,0.00025,0.0013,0.0013,0.00099,0.0013,1,1 +7190000,-0.28,0.026,-0.0064,0.96,-0.047,-0.0082,-0.036,0.00063,0.0038,-0.058,-0.0015,-0.0055,-8.6e-05,3.9e-05,-1e-05,-0.13,-0.1,-0.022,0.51,0.077,-0.03,-0.067,0,0,0.0012,0.0012,0.054,0.17,0.17,0.029,0.16,0.16,0.065,6.6e-05,6.6e-05,4.6e-06,0.04,0.04,0.0023,0.0013,0.00018,0.0013,0.0013,0.00094,0.0013,1,1 +7290000,-0.28,0.026,-0.0064,0.96,-0.07,-0.017,-0.033,-0.0034,0.0023,-0.054,-0.0015,-0.0054,-8.5e-05,0.00011,-5e-06,-0.13,-0.1,-0.022,0.51,0.077,-0.03,-0.067,0,0,0.0012,0.0012,0.053,0.18,0.18,0.028,0.19,0.19,0.064,6.6e-05,6.6e-05,4.6e-06,0.04,0.04,0.0022,0.0013,0.00015,0.0013,0.0013,0.00093,0.0013,1,1 +7390000,-0.28,0.026,-0.0062,0.96,-0.091,-0.024,-0.031,-0.0098,-8.5e-05,-0.052,-0.0015,-0.0054,-8.4e-05,0.00016,-4e-06,-0.13,-0.1,-0.022,0.51,0.077,-0.03,-0.067,0,0,0.0012,0.0012,0.052,0.19,0.19,0.027,0.22,0.22,0.064,6.6e-05,6.6e-05,4.6e-06,0.04,0.04,0.002,0.0013,0.00013,0.0013,0.0013,0.00092,0.0013,1,1 +7490000,-0.28,0.026,-0.0062,0.96,-0.11,-0.032,-0.025,-0.015,-0.0034,-0.046,-0.0015,-0.0053,-8.1e-05,0.00033,7.2e-07,-0.13,-0.1,-0.022,0.5,0.077,-0.03,-0.068,0,0,0.0012,0.0012,0.051,0.2,0.2,0.026,0.26,0.26,0.063,6.6e-05,6.6e-05,4.6e-06,0.04,0.04,0.0019,0.0013,0.00012,0.0013,0.0013,0.00091,0.0013,1,1 +7590000,-0.28,0.026,-0.0063,0.96,-0.13,-0.041,-0.021,-0.024,-0.0083,-0.04,-0.0015,-0.0052,-8e-05,0.00045,-2.7e-05,-0.13,-0.1,-0.022,0.5,0.078,-0.03,-0.068,0,0,0.0012,0.0012,0.051,0.22,0.21,0.025,0.3,0.3,0.062,6.6e-05,6.5e-05,4.6e-06,0.04,0.04,0.0018,0.0013,0.00011,0.0013,0.0013,0.00091,0.0013,1,1 +7690000,-0.28,0.026,-0.0063,0.96,-0.16,-0.051,-0.02,-0.034,-0.015,-0.036,-0.0014,-0.0051,-7.9e-05,0.00056,-6.8e-05,-0.13,-0.1,-0.022,0.5,0.078,-0.03,-0.068,0,0,0.0013,0.0012,0.05,0.23,0.23,0.025,0.34,0.34,0.062,6.5e-05,6.5e-05,4.6e-06,0.04,0.04,0.0017,0.0013,0.0001,0.0013,0.0013,0.0009,0.0013,1,1 +7790000,-0.28,0.026,-0.0062,0.96,-0.18,-0.061,-0.022,-0.041,-0.025,-0.041,-0.0014,-0.005,-7.8e-05,0.00088,-0.00023,-0.13,-0.1,-0.022,0.5,0.078,-0.03,-0.068,0,0,0.0013,0.0012,0.05,0.26,0.25,0.024,0.39,0.39,0.061,6.5e-05,6.5e-05,4.6e-06,0.04,0.04,0.0016,0.0013,9.8e-05,0.0013,0.0013,0.0009,0.0013,1,1 +7890000,-0.28,0.026,-0.0062,0.96,-0.2,-0.071,-0.022,-0.057,-0.033,-0.044,-0.0013,-0.0049,-7.7e-05,0.00099,-0.00028,-0.13,-0.1,-0.022,0.5,0.078,-0.03,-0.068,0,0,0.0013,0.0013,0.05,0.28,0.28,0.023,0.45,0.45,0.06,6.4e-05,6.4e-05,4.6e-06,0.04,0.04,0.0015,0.0013,9.4e-05,0.0013,0.0013,0.0009,0.0013,1,1 +7990000,-0.28,0.026,-0.0061,0.96,-0.22,-0.078,-0.018,-0.08,-0.039,-0.04,-0.0013,-0.0049,-7.7e-05,0.00089,-0.00023,-0.13,-0.1,-0.022,0.5,0.078,-0.03,-0.068,0,0,0.0013,0.0013,0.05,0.3,0.3,0.022,0.51,0.51,0.059,6.3e-05,6.3e-05,4.6e-06,0.04,0.04,0.0015,0.0013,9.1e-05,0.0013,0.0013,0.0009,0.0013,1,1 +8090000,-0.28,0.026,-0.006,0.96,-0.25,-0.09,-0.019,-0.1,-0.053,-0.043,-0.0013,-0.0049,-8e-05,0.00093,-0.00038,-0.13,-0.1,-0.022,0.5,0.078,-0.03,-0.068,0,0,0.0013,0.0013,0.05,0.33,0.33,0.022,0.57,0.57,0.059,6.2e-05,6.2e-05,4.6e-06,0.04,0.04,0.0014,0.0013,8.9e-05,0.0013,0.0013,0.0009,0.0013,1,1 +8190000,-0.28,0.026,-0.0061,0.96,-0.27,-0.1,-0.014,-0.12,-0.069,-0.036,-0.0012,-0.0048,-8.2e-05,0.00096,-0.0005,-0.13,-0.1,-0.022,0.5,0.078,-0.03,-0.068,0,0,0.0013,0.0013,0.049,0.36,0.36,0.021,0.64,0.64,0.058,6.1e-05,6.1e-05,4.6e-06,0.04,0.04,0.0013,0.0013,8.7e-05,0.0013,0.0013,0.0009,0.0013,1,1 +8290000,-0.28,0.026,-0.0061,0.96,-0.3,-0.11,-0.013,-0.16,-0.077,-0.036,-0.0012,-0.005,-8.4e-05,0.0007,-0.00042,-0.13,-0.1,-0.022,0.5,0.078,-0.03,-0.068,0,0,0.0013,0.0013,0.049,0.4,0.39,0.02,0.72,0.72,0.057,6e-05,6e-05,4.6e-06,0.04,0.04,0.0013,0.0013,8.5e-05,0.0013,0.0013,0.0009,0.0013,1,1 +8390000,-0.28,0.026,-0.0061,0.96,-0.33,-0.12,-0.011,-0.2,-0.089,-0.034,-0.0012,-0.005,-8.5e-05,0.00063,-0.00041,-0.13,-0.1,-0.022,0.5,0.078,-0.03,-0.068,0,0,0.0014,0.0013,0.049,0.43,0.43,0.02,0.81,0.8,0.057,5.9e-05,5.9e-05,4.6e-06,0.04,0.04,0.0012,0.0013,8.3e-05,0.0013,0.0013,0.00089,0.0013,1,1 +8490000,-0.28,0.026,-0.0059,0.96,-0.35,-0.12,-0.012,-0.23,-0.094,-0.039,-0.0013,-0.005,-8.2e-05,0.00062,-0.00029,-0.13,-0.1,-0.022,0.5,0.078,-0.03,-0.068,0,0,0.0014,0.0013,0.049,0.47,0.46,0.019,0.9,0.9,0.056,5.8e-05,5.7e-05,4.6e-06,0.04,0.04,0.0011,0.0013,8.2e-05,0.0013,0.0013,0.00089,0.0013,1,1 +8590000,-0.28,0.027,-0.0059,0.96,-0.38,-0.14,-0.0073,-0.26,-0.12,-0.034,-0.0012,-0.0049,-8.2e-05,0.00079,-0.00042,-0.14,-0.1,-0.022,0.5,0.078,-0.03,-0.068,0,0,0.0014,0.0013,0.049,0.51,0.51,0.019,1,1,0.055,5.6e-05,5.5e-05,4.6e-06,0.04,0.04,0.0011,0.0013,8.1e-05,0.0013,0.0013,0.00089,0.0013,1,1 +8690000,-0.28,0.027,-0.0059,0.96,-0.4,-0.14,-0.0089,-0.3,-0.13,-0.035,-0.0012,-0.0049,-8.2e-05,0.00071,-0.00035,-0.14,-0.1,-0.022,0.5,0.078,-0.03,-0.068,0,0,0.0014,0.0014,0.049,0.56,0.55,0.018,1.1,1.1,0.055,5.5e-05,5.4e-05,4.6e-06,0.04,0.04,0.001,0.0013,8e-05,0.0013,0.0013,0.00089,0.0013,1,1 +8790000,-0.28,0.027,-0.0059,0.96,-0.43,-0.15,-0.0084,-0.35,-0.14,-0.032,-0.0012,-0.005,-8.5e-05,0.00052,-0.0004,-0.14,-0.1,-0.022,0.5,0.078,-0.03,-0.068,0,0,0.0014,0.0014,0.049,0.61,0.6,0.018,1.2,1.2,0.055,5.3e-05,5.2e-05,4.6e-06,0.04,0.04,0.00099,0.0013,7.9e-05,0.0013,0.0013,0.00089,0.0013,1,1 +8890000,-0.28,0.027,-0.0059,0.96,-0.46,-0.16,-0.0039,-0.38,-0.16,-0.026,-0.0012,-0.0049,-8.1e-05,0.00064,-0.00031,-0.14,-0.1,-0.022,0.5,0.078,-0.03,-0.068,0,0,0.0014,0.0014,0.049,0.66,0.65,0.017,1.4,1.4,0.054,5.1e-05,5e-05,4.5e-06,0.04,0.04,0.00095,0.0013,7.8e-05,0.0013,0.0013,0.00089,0.0013,1,1 +8990000,-0.28,0.027,-0.0058,0.96,-0.47,-0.17,-0.0028,-0.41,-0.17,-0.029,-0.0013,-0.0047,-7.5e-05,0.001,-0.0003,-0.14,-0.1,-0.022,0.5,0.078,-0.03,-0.068,0,0,0.0014,0.0014,0.049,0.72,0.7,0.017,1.5,1.5,0.054,4.9e-05,4.8e-05,4.5e-06,0.04,0.04,0.00091,0.0013,7.7e-05,0.0013,0.0013,0.00089,0.0013,1,1 +9090000,-0.28,0.027,-0.0056,0.96,-0.5,-0.18,-0.0038,-0.47,-0.17,-0.029,-0.0014,-0.0048,-7.2e-05,0.00081,-2.1e-05,-0.14,-0.1,-0.022,0.5,0.078,-0.03,-0.068,0,0,0.0015,0.0014,0.049,0.77,0.76,0.016,1.7,1.7,0.053,4.7e-05,4.6e-05,4.5e-06,0.04,0.04,0.00087,0.0013,7.6e-05,0.0013,0.0013,0.00089,0.0013,1,1 +9190000,-0.28,0.027,-0.0055,0.96,-0.53,-0.18,-0.0029,-0.52,-0.18,-0.029,-0.0015,-0.0048,-6.9e-05,0.0008,0.00012,-0.14,-0.1,-0.022,0.5,0.078,-0.03,-0.068,0,0,0.0015,0.0014,0.049,0.84,0.82,0.016,1.9,1.9,0.052,4.5e-05,4.4e-05,4.5e-06,0.04,0.04,0.00084,0.0013,7.6e-05,0.0013,0.0013,0.00089,0.0013,1,1 +9290000,-0.28,0.027,-0.0054,0.96,-0.56,-0.19,-0.0011,-0.56,-0.21,-0.026,-0.0014,-0.0047,-6.7e-05,0.00094,9e-05,-0.14,-0.1,-0.022,0.5,0.078,-0.03,-0.068,0,0,0.0015,0.0014,0.049,0.9,0.88,0.016,2.1,2.1,0.052,4.3e-05,4.2e-05,4.5e-06,0.04,0.04,0.0008,0.0013,7.5e-05,0.0013,0.0013,0.00089,0.0013,1,1 +9390000,-0.28,0.027,-0.0054,0.96,-0.023,-0.0084,0.0003,-0.57,-0.21,-0.026,-0.0013,-0.0047,-7.1e-05,0.00094,9e-05,-0.14,-0.1,-0.022,0.5,0.078,-0.03,-0.068,0,0,0.0015,0.0014,0.049,25,25,0.015,1e+02,1e+02,0.052,4.2e-05,4e-05,4.5e-06,0.04,0.04,0.00077,0.0013,7.4e-05,0.0013,0.0013,0.00089,0.0013,1,1 +9490000,-0.28,0.027,-0.0053,0.96,-0.049,-0.017,0.0021,-0.58,-0.21,-0.023,-0.0014,-0.0048,-7.1e-05,0.00094,9e-05,-0.14,-0.1,-0.022,0.5,0.078,-0.03,-0.068,0,0,0.0015,0.0014,0.049,25,25,0.015,1e+02,1e+02,0.051,4e-05,3.8e-05,4.5e-06,0.04,0.04,0.00074,0.0013,7.4e-05,0.0013,0.0013,0.00089,0.0013,1,1 +9590000,-0.28,0.027,-0.0056,0.96,-0.076,-0.023,0.0023,-0.58,-0.21,-0.024,-0.0012,-0.0048,-8e-05,0.00094,9e-05,-0.14,-0.1,-0.022,0.5,0.078,-0.03,-0.068,0,0,0.0015,0.0014,0.049,25,25,0.015,51,51,0.05,3.8e-05,3.7e-05,4.5e-06,0.04,0.04,0.00072,0.0013,7.3e-05,0.0013,0.0013,0.00089,0.0013,1,1 +9690000,-0.28,0.027,-0.0057,0.96,-0.1,-0.031,0.0054,-0.59,-0.21,-0.023,-0.0012,-0.0048,-8.2e-05,0.00094,9e-05,-0.14,-0.1,-0.022,0.5,0.078,-0.03,-0.068,0,0,0.0015,0.0015,0.049,25,25,0.014,52,52,0.05,3.6e-05,3.5e-05,4.5e-06,0.04,0.04,0.00069,0.0013,7.3e-05,0.0013,0.0013,0.00089,0.0013,1,1 +9790000,-0.28,0.027,-0.0059,0.96,-0.13,-0.038,0.0041,-0.59,-0.22,-0.023,-0.001,-0.005,-9.1e-05,0.00094,9e-05,-0.14,-0.1,-0.022,0.5,0.078,-0.03,-0.068,0,0,0.0015,0.0015,0.049,25,25,0.014,35,35,0.05,3.4e-05,3.3e-05,4.5e-06,0.04,0.04,0.00067,0.0013,7.3e-05,0.0013,0.0013,0.00089,0.0013,1,1 +9890000,-0.28,0.027,-0.0058,0.96,-0.16,-0.047,0.0057,-0.61,-0.22,-0.023,-0.0011,-0.0049,-8.9e-05,0.00094,9e-05,-0.14,-0.1,-0.022,0.5,0.078,-0.03,-0.068,0,0,0.0015,0.0015,0.049,25,25,0.014,37,37,0.049,3.2e-05,3.1e-05,4.5e-06,0.04,0.04,0.00064,0.0013,7.2e-05,0.0013,0.0013,0.00089,0.0013,1,1 +9990000,-0.28,0.027,-0.006,0.96,-0.18,-0.051,0.0065,-0.61,-0.22,-0.025,-0.00096,-0.005,-9.6e-05,0.00094,9e-05,-0.14,-0.1,-0.022,0.5,0.078,-0.03,-0.068,0,0,0.0015,0.0015,0.049,24,24,0.014,28,28,0.049,3.1e-05,3e-05,4.5e-06,0.04,0.04,0.00062,0.0013,7.2e-05,0.0013,0.0013,0.00089,0.0013,1,1 +10090000,-0.28,0.027,-0.0061,0.96,-0.21,-0.054,0.0078,-0.63,-0.23,-0.023,-0.00084,-0.0051,-0.0001,0.00094,9e-05,-0.14,-0.1,-0.022,0.5,0.078,-0.03,-0.068,0,0,0.0015,0.0015,0.049,24,24,0.013,30,30,0.049,2.9e-05,2.8e-05,4.5e-06,0.04,0.04,0.0006,0.0013,7.1e-05,0.0013,0.0013,0.00089,0.0013,1,1 +10190000,-0.28,0.027,-0.0064,0.96,-0.23,-0.056,0.0087,-0.64,-0.23,-0.024,-0.00067,-0.005,-0.00011,0.00094,9e-05,-0.14,-0.1,-0.022,0.5,0.078,-0.03,-0.068,0,0,0.0015,0.0015,0.049,23,23,0.013,25,25,0.048,2.8e-05,2.7e-05,4.5e-06,0.04,0.04,0.00058,0.0013,7.1e-05,0.0013,0.0013,0.00089,0.0013,1,1 +10290000,-0.28,0.027,-0.0064,0.96,-0.26,-0.065,0.0079,-0.66,-0.23,-0.023,-0.00072,-0.005,-0.00011,0.00094,9e-05,-0.14,-0.1,-0.022,0.5,0.078,-0.03,-0.068,0,0,0.0015,0.0015,0.049,23,23,0.013,27,27,0.048,2.6e-05,2.5e-05,4.5e-06,0.04,0.04,0.00057,0.0013,7.1e-05,0.0013,0.0013,0.00089,0.0013,1,1 +10390000,-0.28,0.028,-0.0063,0.96,-0.012,-0.0081,-0.0025,-7.9e-05,-0.00028,-0.023,-0.00074,-0.0049,-0.0001,0.001,0.00015,-0.14,-0.1,-0.022,0.5,0.078,-0.03,-0.068,0,0,0.0015,0.0015,0.049,0.25,0.25,0.56,0.25,0.25,0.049,2.5e-05,2.4e-05,4.5e-06,0.04,0.04,0.00055,0.0013,7.1e-05,0.0013,0.0013,0.00089,0.0013,1,1 +10490000,-0.28,0.027,-0.0064,0.96,-0.041,-0.014,0.0066,-0.0027,-0.0013,-0.018,-0.00066,-0.005,-0.00011,0.00082,2.7e-05,-0.14,-0.1,-0.022,0.5,0.078,-0.03,-0.068,0,0,0.0015,0.0015,0.049,0.26,0.26,0.55,0.26,0.26,0.058,2.4e-05,2.3e-05,4.5e-06,0.04,0.04,0.00054,0.0013,7e-05,0.0013,0.0013,0.00089,0.0013,1,1 +10590000,-0.28,0.027,-0.0063,0.96,-0.052,-0.013,0.012,-0.0032,-0.00096,-0.016,-0.00073,-0.005,-0.0001,0.0014,7.7e-05,-0.14,-0.1,-0.022,0.5,0.078,-0.03,-0.068,0,0,0.0015,0.0015,0.049,0.13,0.13,0.27,0.26,0.26,0.056,2.2e-05,2.1e-05,4.5e-06,0.04,0.04,0.00052,0.0013,7e-05,0.0013,0.0013,0.00089,0.0013,1,1 +10690000,-0.28,0.027,-0.0063,0.96,-0.081,-0.018,0.015,-0.0099,-0.0025,-0.013,-0.0007,-0.005,-0.00011,0.0014,2.4e-05,-0.14,-0.1,-0.022,0.5,0.078,-0.03,-0.068,0,0,0.0015,0.0015,0.049,0.14,0.14,0.26,0.27,0.27,0.065,2.1e-05,2e-05,4.5e-06,0.04,0.04,0.00052,0.0013,7e-05,0.0013,0.0013,0.00089,0.0013,1,1 +10790000,-0.28,0.027,-0.0062,0.96,-0.079,-0.021,0.013,-7.5e-06,-0.0018,-0.012,-0.00072,-0.005,-0.00011,0.0035,-0.0011,-0.14,-0.1,-0.022,0.5,0.078,-0.03,-0.069,0,0,0.0015,0.0014,0.049,0.095,0.095,0.17,0.13,0.13,0.062,2e-05,1.9e-05,4.5e-06,0.039,0.039,0.00051,0.0013,6.9e-05,0.0013,0.0013,0.00088,0.0013,1,1 +10890000,-0.28,0.027,-0.006,0.96,-0.11,-0.03,0.0093,-0.0092,-0.0044,-0.015,-0.00083,-0.005,-0.0001,0.0036,-0.00084,-0.14,-0.1,-0.022,0.5,0.078,-0.03,-0.069,0,0,0.0015,0.0014,0.049,0.11,0.11,0.16,0.14,0.14,0.068,1.9e-05,1.8e-05,4.5e-06,0.039,0.039,0.0005,0.0013,6.9e-05,0.0013,0.0013,0.00088,0.0013,1,1 +10990000,-0.28,0.026,-0.006,0.96,-0.096,-0.031,0.016,-0.0034,-0.0023,-0.0093,-0.00085,-0.0052,-0.00011,0.0077,-0.0021,-0.14,-0.1,-0.023,0.5,0.078,-0.03,-0.069,0,0,0.0014,0.0013,0.049,0.083,0.083,0.12,0.091,0.091,0.065,1.8e-05,1.7e-05,4.5e-06,0.039,0.039,0.0005,0.0013,6.9e-05,0.0013,0.0013,0.00088,0.0013,1,1 +11090000,-0.28,0.027,-0.0064,0.96,-0.12,-0.039,0.019,-0.014,-0.0055,-0.0055,-0.00072,-0.0052,-0.00011,0.0078,-0.0025,-0.14,-0.1,-0.023,0.5,0.078,-0.03,-0.069,0,0,0.0014,0.0013,0.049,0.096,0.097,0.11,0.097,0.097,0.069,1.7e-05,1.6e-05,4.5e-06,0.039,0.039,0.00049,0.0013,6.9e-05,0.0013,0.0013,0.00088,0.0013,1,1 +11190000,-0.28,0.025,-0.0065,0.96,-0.1,-0.034,0.026,-0.0065,-0.0031,0.00085,-0.00071,-0.0053,-0.00011,0.014,-0.0046,-0.14,-0.1,-0.023,0.5,0.079,-0.031,-0.069,0,0,0.0013,0.0012,0.049,0.079,0.079,0.083,0.072,0.072,0.066,1.6e-05,1.5e-05,4.5e-06,0.038,0.038,0.00049,0.0013,6.8e-05,0.0013,0.0012,0.00087,0.0013,1,1 +11290000,-0.28,0.025,-0.0066,0.96,-0.13,-0.038,0.025,-0.018,-0.0066,0.00091,-0.00071,-0.0054,-0.00012,0.014,-0.0046,-0.14,-0.1,-0.023,0.5,0.079,-0.031,-0.069,0,0,0.0013,0.0012,0.049,0.094,0.094,0.077,0.078,0.078,0.069,1.5e-05,1.4e-05,4.5e-06,0.038,0.038,0.00049,0.0013,6.8e-05,0.0013,0.0012,0.00087,0.0013,1,1 +11390000,-0.28,0.024,-0.0065,0.96,-0.11,-0.032,0.016,-0.01,-0.004,-0.008,-0.00075,-0.0055,-0.00012,0.021,-0.0065,-0.14,-0.1,-0.023,0.5,0.079,-0.031,-0.069,0,0,0.0012,0.0011,0.048,0.078,0.078,0.062,0.062,0.062,0.066,1.4e-05,1.4e-05,4.5e-06,0.037,0.037,0.00048,0.0013,6.8e-05,0.0013,0.0012,0.00085,0.0013,1,1 +11490000,-0.28,0.024,-0.0064,0.96,-0.13,-0.036,0.021,-0.022,-0.0074,-0.002,-0.00075,-0.0055,-0.00012,0.021,-0.0065,-0.14,-0.1,-0.023,0.5,0.079,-0.031,-0.069,0,0,0.0012,0.0011,0.048,0.093,0.093,0.057,0.069,0.069,0.067,1.3e-05,1.3e-05,4.5e-06,0.037,0.037,0.00048,0.0013,6.7e-05,0.0013,0.0012,0.00085,0.0013,1,1 +11590000,-0.28,0.023,-0.0065,0.96,-0.11,-0.029,0.019,-0.013,-0.0046,-0.0033,-0.00079,-0.0056,-0.00012,0.027,-0.0085,-0.14,-0.1,-0.023,0.5,0.08,-0.031,-0.069,0,0,0.001,0.00098,0.048,0.077,0.077,0.048,0.056,0.056,0.065,1.3e-05,1.2e-05,4.5e-06,0.036,0.036,0.00048,0.0012,6.7e-05,0.0013,0.0012,0.00084,0.0013,1,1 +11690000,-0.28,0.022,-0.0065,0.96,-0.12,-0.035,0.019,-0.024,-0.0078,-0.0047,-0.00081,-0.0057,-0.00012,0.027,-0.0083,-0.14,-0.1,-0.023,0.5,0.08,-0.031,-0.069,0,0,0.001,0.00099,0.048,0.092,0.092,0.044,0.063,0.063,0.066,1.2e-05,1.2e-05,4.5e-06,0.036,0.036,0.00048,0.0012,6.7e-05,0.0013,0.0012,0.00084,0.0013,1,1 +11790000,-0.28,0.021,-0.0063,0.96,-0.097,-0.033,0.02,-0.014,-0.0064,-0.0019,-0.0009,-0.0057,-0.00012,0.033,-0.01,-0.14,-0.1,-0.023,0.5,0.081,-0.031,-0.069,0,0,0.00091,0.00087,0.048,0.076,0.076,0.037,0.053,0.053,0.063,1.1e-05,1.1e-05,4.5e-06,0.035,0.035,0.00047,0.0012,6.6e-05,0.0013,0.0012,0.00083,0.0013,1,1 +11890000,-0.28,0.021,-0.0065,0.96,-0.11,-0.037,0.018,-0.024,-0.0097,-0.0012,-0.00088,-0.0058,-0.00012,0.033,-0.01,-0.14,-0.1,-0.023,0.5,0.081,-0.031,-0.07,0,0,0.00091,0.00087,0.048,0.089,0.089,0.034,0.06,0.06,0.063,1.1e-05,1e-05,4.5e-06,0.035,0.035,0.00047,0.0012,6.6e-05,0.0013,0.0012,0.00083,0.0013,1,1 +11990000,-0.28,0.02,-0.0066,0.96,-0.091,-0.028,0.015,-0.017,-0.0064,-0.005,-0.0009,-0.0059,-0.00012,0.039,-0.011,-0.14,-0.11,-0.023,0.5,0.081,-0.031,-0.07,0,0,0.00081,0.00078,0.047,0.076,0.076,0.03,0.062,0.062,0.061,1e-05,9.8e-06,4.5e-06,0.034,0.034,0.00047,0.0012,6.6e-05,0.0013,0.0012,0.00082,0.0013,1,1 +12090000,-0.28,0.02,-0.0066,0.96,-0.1,-0.03,0.019,-0.027,-0.0092,0.0011,-0.00086,-0.0058,-0.00012,0.039,-0.012,-0.14,-0.11,-0.023,0.5,0.081,-0.031,-0.07,0,0,0.00081,0.00078,0.047,0.089,0.088,0.027,0.071,0.071,0.06,9.8e-06,9.3e-06,4.5e-06,0.034,0.034,0.00047,0.0012,6.6e-05,0.0013,0.0012,0.00081,0.0013,1,1 +12190000,-0.28,0.019,-0.0067,0.96,-0.084,-0.018,0.018,-0.014,-0.0032,0.0029,-0.00082,-0.0059,-0.00012,0.045,-0.013,-0.14,-0.11,-0.023,0.5,0.082,-0.031,-0.07,0,0,0.00072,0.0007,0.047,0.07,0.07,0.024,0.057,0.057,0.058,9.2e-06,8.8e-06,4.5e-06,0.033,0.033,0.00047,0.0012,6.5e-05,0.0012,0.0012,0.0008,0.0012,1,1 +12290000,-0.28,0.019,-0.0068,0.96,-0.09,-0.017,0.017,-0.023,-0.0048,0.004,-0.00079,-0.0059,-0.00012,0.045,-0.014,-0.14,-0.11,-0.023,0.5,0.082,-0.031,-0.07,0,0,0.00072,0.0007,0.047,0.081,0.081,0.022,0.065,0.065,0.058,8.9e-06,8.5e-06,4.5e-06,0.033,0.033,0.00047,0.0012,6.5e-05,0.0012,0.0012,0.0008,0.0012,1,1 +12390000,-0.28,0.019,-0.0068,0.96,-0.073,-0.012,0.015,-0.013,-0.0027,-0.002,-0.00078,-0.0059,-0.00012,0.048,-0.015,-0.14,-0.11,-0.023,0.5,0.082,-0.031,-0.07,0,0,0.00066,0.00063,0.047,0.066,0.065,0.02,0.054,0.054,0.056,8.4e-06,8e-06,4.5e-06,0.033,0.033,0.00047,0.0012,6.5e-05,0.0012,0.0012,0.00079,0.0012,1,1 +12490000,-0.28,0.019,-0.0069,0.96,-0.081,-0.013,0.019,-0.021,-0.004,9.6e-05,-0.00076,-0.0059,-0.00013,0.048,-0.016,-0.14,-0.11,-0.023,0.5,0.082,-0.032,-0.07,0,0,0.00066,0.00064,0.047,0.075,0.075,0.018,0.062,0.062,0.055,8.1e-06,7.7e-06,4.5e-06,0.033,0.033,0.00047,0.0012,6.5e-05,0.0012,0.0012,0.00079,0.0012,1,1 +12590000,-0.28,0.018,-0.0068,0.96,-0.065,-0.011,0.021,-0.01,-0.0039,0.0019,-0.00076,-0.0059,-0.00013,0.051,-0.018,-0.14,-0.11,-0.023,0.5,0.082,-0.032,-0.07,0,0,0.0006,0.00058,0.047,0.061,0.061,0.017,0.052,0.052,0.054,7.7e-06,7.4e-06,4.5e-06,0.032,0.032,0.00047,0.0012,6.4e-05,0.0012,0.0012,0.00078,0.0012,1,1 +12690000,-0.28,0.018,-0.0067,0.96,-0.072,-0.0098,0.02,-0.017,-0.0049,0.0035,-0.00075,-0.0059,-0.00013,0.051,-0.018,-0.14,-0.11,-0.023,0.5,0.082,-0.032,-0.07,0,0,0.0006,0.00059,0.047,0.069,0.069,0.015,0.059,0.059,0.053,7.4e-06,7e-06,4.5e-06,0.032,0.032,0.00047,0.0012,6.4e-05,0.0012,0.0012,0.00078,0.0012,1,1 +12790000,-0.28,0.018,-0.0065,0.96,-0.056,-0.015,0.022,-0.01,-0.0078,0.0057,-0.00082,-0.0059,-0.00013,0.054,-0.019,-0.14,-0.11,-0.023,0.5,0.082,-0.032,-0.07,0,0,0.00056,0.00055,0.047,0.061,0.061,0.014,0.061,0.061,0.051,7.1e-06,6.7e-06,4.5e-06,0.032,0.032,0.00046,0.0012,6.4e-05,0.0012,0.0012,0.00078,0.0012,1,1 +12890000,-0.28,0.018,-0.0065,0.96,-0.061,-0.016,0.023,-0.016,-0.0096,0.0088,-0.00084,-0.0059,-0.00012,0.054,-0.019,-0.14,-0.11,-0.023,0.5,0.082,-0.032,-0.07,0,0,0.00056,0.00055,0.047,0.069,0.069,0.013,0.07,0.07,0.051,6.8e-06,6.5e-06,4.5e-06,0.032,0.032,0.00046,0.0012,6.4e-05,0.0012,0.0012,0.00077,0.0012,1,1 +12990000,-0.28,0.017,-0.0065,0.96,-0.05,-0.013,0.023,-0.008,-0.0066,0.01,-0.00088,-0.0059,-0.00012,0.056,-0.019,-0.14,-0.11,-0.023,0.5,0.083,-0.032,-0.07,0,0,0.00053,0.00052,0.047,0.055,0.055,0.012,0.056,0.056,0.05,6.5e-06,6.2e-06,4.5e-06,0.032,0.032,0.00046,0.0012,6.4e-05,0.0012,0.0012,0.00077,0.0012,1,1 +13090000,-0.28,0.017,-0.0064,0.96,-0.054,-0.015,0.021,-0.013,-0.0083,0.0089,-0.00091,-0.0059,-0.00012,0.056,-0.018,-0.14,-0.11,-0.023,0.5,0.083,-0.032,-0.07,0,0,0.00053,0.00052,0.046,0.062,0.062,0.011,0.065,0.065,0.049,6.3e-06,5.9e-06,4.5e-06,0.032,0.032,0.00046,0.0012,6.4e-05,0.0012,0.0012,0.00077,0.0012,1,1 +13190000,-0.28,0.017,-0.0063,0.96,-0.046,-0.015,0.02,-0.0097,-0.0091,0.0096,-0.00094,-0.0059,-0.00012,0.058,-0.019,-0.14,-0.11,-0.023,0.5,0.083,-0.032,-0.07,0,0,0.00051,0.00049,0.046,0.055,0.055,0.011,0.066,0.066,0.047,6e-06,5.7e-06,4.5e-06,0.031,0.031,0.00046,0.0012,6.3e-05,0.0012,0.0012,0.00076,0.0012,1,1 +13290000,-0.28,0.017,-0.0064,0.96,-0.049,-0.015,0.017,-0.015,-0.011,0.009,-0.00092,-0.0058,-0.00012,0.058,-0.019,-0.14,-0.11,-0.023,0.5,0.083,-0.032,-0.07,0,0,0.00051,0.00049,0.046,0.061,0.061,0.01,0.075,0.075,0.047,5.8e-06,5.5e-06,4.5e-06,0.031,0.031,0.00046,0.0012,6.3e-05,0.0012,0.0012,0.00076,0.0012,1,1 +13390000,-0.28,0.017,-0.0064,0.96,-0.04,-0.011,0.017,-0.0074,-0.007,0.0097,-0.0009,-0.0059,-0.00012,0.059,-0.02,-0.14,-0.11,-0.023,0.5,0.083,-0.032,-0.07,0,0,0.00048,0.00047,0.046,0.049,0.049,0.0094,0.06,0.06,0.046,5.6e-06,5.2e-06,4.5e-06,0.031,0.031,0.00046,0.0012,6.3e-05,0.0012,0.0012,0.00076,0.0012,1,1 +13490000,-0.28,0.017,-0.0064,0.96,-0.044,-0.014,0.017,-0.012,-0.0083,0.006,-0.0009,-0.0059,-0.00012,0.059,-0.02,-0.14,-0.11,-0.023,0.5,0.083,-0.032,-0.07,0,0,0.00048,0.00047,0.046,0.054,0.054,0.009,0.068,0.068,0.045,5.4e-06,5e-06,4.5e-06,0.031,0.031,0.00046,0.0012,6.3e-05,0.0012,0.0012,0.00076,0.0012,1,1 +13590000,-0.28,0.017,-0.0064,0.96,-0.035,-0.01,0.018,-0.0035,-0.0057,0.0045,-0.00088,-0.0059,-0.00012,0.061,-0.02,-0.14,-0.11,-0.023,0.5,0.083,-0.032,-0.07,0,0,0.00047,0.00045,0.046,0.045,0.045,0.0085,0.055,0.055,0.044,5.2e-06,4.9e-06,4.5e-06,0.031,0.031,0.00046,0.0012,6.3e-05,0.0012,0.0012,0.00075,0.0012,1,1 +13690000,-0.28,0.017,-0.0063,0.96,-0.037,-0.0095,0.019,-0.0071,-0.0067,0.0072,-0.00089,-0.0059,-0.00012,0.061,-0.02,-0.14,-0.11,-0.023,0.5,0.083,-0.032,-0.07,0,0,0.00047,0.00046,0.046,0.049,0.049,0.0082,0.063,0.063,0.044,5e-06,4.7e-06,4.5e-06,0.031,0.031,0.00046,0.0012,6.3e-05,0.0012,0.0012,0.00075,0.0012,1,1 +13790000,-0.28,0.016,-0.0064,0.96,-0.029,-0.0071,0.019,0.00061,-0.0035,0.0068,-0.00089,-0.0059,-0.00012,0.062,-0.021,-0.14,-0.11,-0.023,0.5,0.083,-0.032,-0.07,0,0,0.00045,0.00044,0.046,0.041,0.041,0.0078,0.052,0.052,0.042,4.8e-06,4.5e-06,4.5e-06,0.031,0.031,0.00046,0.0012,6.3e-05,0.0012,0.0012,0.00075,0.0012,1,1 +13890000,-0.28,0.016,-0.0063,0.96,-0.031,-0.0086,0.02,-0.0022,-0.0044,0.0091,-0.00092,-0.0059,-0.00012,0.061,-0.02,-0.14,-0.11,-0.023,0.5,0.083,-0.032,-0.07,0,0,0.00045,0.00044,0.046,0.045,0.045,0.0076,0.059,0.059,0.042,4.7e-06,4.4e-06,4.5e-06,0.031,0.031,0.00046,0.0012,6.2e-05,0.0012,0.0012,0.00075,0.0012,1,1 +13990000,-0.28,0.016,-0.0062,0.96,-0.03,-0.012,0.018,-0.00073,-0.0048,0.008,-0.00094,-0.0059,-0.00012,0.062,-0.02,-0.14,-0.11,-0.023,0.5,0.083,-0.032,-0.07,0,0,0.00044,0.00043,0.046,0.039,0.039,0.0073,0.05,0.05,0.041,4.5e-06,4.2e-06,4.5e-06,0.031,0.031,0.00046,0.0012,6.2e-05,0.0012,0.0012,0.00074,0.0012,1,1 +14090000,-0.28,0.016,-0.0064,0.96,-0.031,-0.0062,0.02,-0.004,-0.0053,0.0046,-0.00087,-0.0059,-0.00012,0.062,-0.021,-0.14,-0.11,-0.023,0.5,0.083,-0.032,-0.07,0,0,0.00044,0.00043,0.046,0.042,0.042,0.0072,0.057,0.057,0.041,4.4e-06,4e-06,4.5e-06,0.031,0.031,0.00046,0.0012,6.2e-05,0.0012,0.0012,0.00074,0.0012,1,1 +14190000,-0.28,0.016,-0.0064,0.96,-0.025,-0.0047,0.02,-0.00094,-0.004,0.0049,-0.00082,-0.0059,-0.00013,0.063,-0.022,-0.14,-0.11,-0.023,0.5,0.083,-0.032,-0.07,0,0,0.00043,0.00042,0.046,0.036,0.036,0.007,0.049,0.049,0.04,4.2e-06,3.9e-06,4.5e-06,0.031,0.03,0.00046,0.0012,6.2e-05,0.0012,0.0012,0.00074,0.0012,1,1 +14290000,-0.28,0.016,-0.0064,0.96,-0.028,-0.0051,0.018,-0.0036,-0.0044,0.0092,-0.00082,-0.0059,-0.00013,0.063,-0.022,-0.14,-0.11,-0.023,0.5,0.083,-0.032,-0.07,0,0,0.00043,0.00042,0.046,0.04,0.04,0.0069,0.055,0.055,0.039,4.1e-06,3.8e-06,4.5e-06,0.031,0.03,0.00046,0.0012,6.2e-05,0.0012,0.0012,0.00074,0.0012,1,1 +14390000,-0.28,0.016,-0.0064,0.96,-0.026,-0.0045,0.019,-0.00096,-0.0049,0.014,-0.00083,-0.0059,-0.00012,0.064,-0.022,-0.14,-0.11,-0.023,0.5,0.083,-0.032,-0.07,0,0,0.00042,0.00041,0.046,0.034,0.034,0.0067,0.048,0.048,0.039,4e-06,3.6e-06,4.5e-06,0.031,0.03,0.00046,0.0012,6.2e-05,0.0012,0.0012,0.00074,0.0012,1,1 +14490000,-0.28,0.016,-0.0066,0.96,-0.026,-0.0039,0.023,-0.0037,-0.005,0.016,-0.00079,-0.0059,-0.00013,0.064,-0.023,-0.14,-0.11,-0.023,0.5,0.083,-0.032,-0.07,0,0,0.00042,0.00042,0.046,0.037,0.037,0.0066,0.054,0.054,0.038,3.8e-06,3.5e-06,4.5e-06,0.031,0.03,0.00046,0.0012,6.2e-05,0.0012,0.0012,0.00074,0.0012,1,1 +14590000,-0.28,0.016,-0.0067,0.96,-0.028,-0.0054,0.021,-0.0038,-0.0053,0.012,-0.00078,-0.0058,-0.00013,0.064,-0.023,-0.14,-0.11,-0.023,0.5,0.083,-0.032,-0.07,0,0,0.00042,0.00041,0.046,0.032,0.032,0.0065,0.047,0.047,0.038,3.7e-06,3.4e-06,4.5e-06,0.03,0.03,0.00046,0.0012,6.2e-05,0.0012,0.0012,0.00074,0.0012,1,1 +14690000,-0.28,0.016,-0.0067,0.96,-0.029,-0.0058,0.021,-0.0067,-0.006,0.012,-0.00078,-0.0058,-0.00013,0.064,-0.023,-0.14,-0.11,-0.023,0.5,0.083,-0.032,-0.07,0,0,0.00042,0.00041,0.046,0.035,0.035,0.0065,0.053,0.053,0.037,3.6e-06,3.3e-06,4.5e-06,0.03,0.03,0.00046,0.0012,6.2e-05,0.0012,0.0012,0.00074,0.0012,1,1 +14790000,-0.28,0.016,-0.0068,0.96,-0.03,-0.003,0.021,-0.0052,-0.0014,0.015,-0.0008,-0.0058,-0.00012,0.065,-0.023,-0.14,-0.11,-0.023,0.5,0.083,-0.032,-0.07,0,0,0.00041,0.0004,0.046,0.031,0.031,0.0064,0.046,0.046,0.036,3.5e-06,3.2e-06,4.5e-06,0.03,0.03,0.00046,0.0012,6.1e-05,0.0012,0.0012,0.00073,0.0012,1,1 +14890000,-0.28,0.016,-0.0067,0.96,-0.032,-0.0012,0.026,-0.0086,-0.002,0.016,-0.00081,-0.0057,-0.00012,0.066,-0.022,-0.14,-0.11,-0.024,0.5,0.083,-0.032,-0.07,0,0,0.00041,0.00041,0.046,0.033,0.033,0.0064,0.052,0.052,0.036,3.4e-06,3.1e-06,4.5e-06,0.03,0.03,0.00046,0.0012,6.1e-05,0.0012,0.0012,0.00073,0.0012,1,1 +14990000,-0.28,0.016,-0.0067,0.96,-0.031,-0.003,0.028,-0.0069,-0.003,0.018,-0.00081,-0.0057,-0.00012,0.067,-0.023,-0.14,-0.11,-0.024,0.5,0.083,-0.032,-0.07,0,0,0.00041,0.0004,0.046,0.029,0.029,0.0064,0.045,0.045,0.036,3.3e-06,3e-06,4.5e-06,0.03,0.03,0.00046,0.0012,6.1e-05,0.0012,0.0012,0.00073,0.0012,1,1 +15090000,-0.28,0.016,-0.0067,0.96,-0.032,-0.0042,0.032,-0.01,-0.0033,0.021,-0.00081,-0.0057,-0.00012,0.067,-0.023,-0.14,-0.11,-0.024,0.5,0.083,-0.032,-0.07,0,0,0.00041,0.0004,0.046,0.032,0.032,0.0064,0.051,0.051,0.035,3.2e-06,2.9e-06,4.5e-06,0.03,0.03,0.00046,0.0012,6.1e-05,0.0012,0.0012,0.00073,0.0012,1,1 +15190000,-0.28,0.016,-0.0068,0.96,-0.03,-0.0022,0.033,-0.008,-0.0026,0.023,-0.0008,-0.0057,-0.00012,0.068,-0.023,-0.14,-0.11,-0.024,0.5,0.083,-0.032,-0.07,0,0,0.0004,0.0004,0.046,0.028,0.028,0.0064,0.045,0.045,0.035,3.1e-06,2.8e-06,4.5e-06,0.03,0.03,0.00046,0.0012,6.1e-05,0.0012,0.0012,0.00073,0.0012,1,1 +15290000,-0.28,0.016,-0.0069,0.96,-0.034,-0.0024,0.032,-0.012,-0.0032,0.02,-0.00081,-0.0057,-0.00012,0.068,-0.023,-0.14,-0.11,-0.024,0.5,0.083,-0.032,-0.07,0,0,0.0004,0.0004,0.046,0.03,0.03,0.0065,0.05,0.05,0.035,3e-06,2.7e-06,4.5e-06,0.03,0.03,0.00046,0.0012,6.1e-05,0.0012,0.0012,0.00073,0.0012,1,1 +15390000,-0.28,0.016,-0.0069,0.96,-0.032,-0.0042,0.032,-0.0092,-0.0026,0.02,-0.00081,-0.0057,-0.00012,0.068,-0.023,-0.14,-0.11,-0.024,0.5,0.083,-0.032,-0.07,0,0,0.0004,0.00039,0.046,0.026,0.027,0.0064,0.044,0.044,0.034,2.9e-06,2.6e-06,4.5e-06,0.03,0.03,0.00046,0.0012,6.1e-05,0.0012,0.0012,0.00073,0.0012,1,1 +15490000,-0.28,0.016,-0.007,0.96,-0.034,-0.0018,0.032,-0.012,-0.003,0.021,-0.00082,-0.0057,-0.00012,0.068,-0.023,-0.14,-0.11,-0.024,0.5,0.083,-0.032,-0.07,0,0,0.0004,0.0004,0.046,0.028,0.029,0.0065,0.05,0.05,0.034,2.8e-06,2.5e-06,4.5e-06,0.03,0.03,0.00045,0.0012,6.1e-05,0.0012,0.0012,0.00073,0.0012,1,1 +15590000,-0.28,0.016,-0.0069,0.96,-0.03,-0.006,0.032,-0.0077,-0.0061,0.02,-0.00085,-0.0057,-0.00012,0.069,-0.023,-0.14,-0.11,-0.024,0.5,0.083,-0.032,-0.07,0,0,0.0004,0.00039,0.046,0.025,0.025,0.0065,0.044,0.044,0.034,2.7e-06,2.5e-06,4.5e-06,0.03,0.03,0.00045,0.0012,6.1e-05,0.0012,0.0012,0.00073,0.0012,1,1 +15690000,-0.28,0.016,-0.0068,0.96,-0.032,-0.0041,0.032,-0.01,-0.0067,0.021,-0.00089,-0.0057,-0.00012,0.068,-0.023,-0.14,-0.11,-0.024,0.5,0.083,-0.032,-0.07,0,0,0.0004,0.00039,0.046,0.027,0.027,0.0066,0.049,0.049,0.034,2.7e-06,2.4e-06,4.5e-06,0.03,0.03,0.00045,0.0012,6.1e-05,0.0012,0.0012,0.00073,0.0012,1,1 +15790000,-0.28,0.016,-0.0068,0.96,-0.028,-0.0028,0.032,-0.0077,-0.0057,0.023,-0.00092,-0.0057,-0.00012,0.068,-0.022,-0.14,-0.11,-0.024,0.5,0.083,-0.032,-0.07,0,0,0.0004,0.00039,0.046,0.024,0.024,0.0066,0.043,0.044,0.033,2.6e-06,2.3e-06,4.5e-06,0.03,0.03,0.00045,0.0012,6.1e-05,0.0012,0.0012,0.00073,0.0012,1,1 +15890000,-0.28,0.016,-0.0069,0.96,-0.029,-0.0041,0.033,-0.011,-0.0058,0.023,-0.00089,-0.0057,-0.00012,0.068,-0.023,-0.14,-0.11,-0.024,0.5,0.083,-0.032,-0.07,0,0,0.0004,0.00039,0.046,0.026,0.026,0.0067,0.049,0.049,0.034,2.5e-06,2.3e-06,4.5e-06,0.03,0.03,0.00045,0.0012,6.1e-05,0.0012,0.0012,0.00073,0.0012,1,1 +15990000,-0.28,0.016,-0.0068,0.96,-0.027,-0.0035,0.03,-0.0091,-0.0049,0.022,-0.00089,-0.0057,-0.00012,0.068,-0.023,-0.14,-0.11,-0.024,0.5,0.084,-0.032,-0.07,0,0,0.00039,0.00039,0.046,0.023,0.023,0.0068,0.043,0.043,0.033,2.4e-06,2.2e-06,4.5e-06,0.03,0.03,0.00045,0.0012,6.1e-05,0.0012,0.0012,0.00072,0.0012,1,1 +16090000,-0.28,0.016,-0.0068,0.96,-0.028,-0.0023,0.028,-0.012,-0.005,0.022,-0.00087,-0.0057,-0.00012,0.069,-0.023,-0.14,-0.11,-0.024,0.5,0.084,-0.032,-0.07,0,0,0.00039,0.00039,0.046,0.025,0.025,0.0069,0.048,0.048,0.033,2.4e-06,2.1e-06,4.5e-06,0.03,0.03,0.00045,0.0012,6e-05,0.0012,0.0012,0.00072,0.0012,1,1 +16190000,-0.28,0.016,-0.0068,0.96,-0.026,-0.002,0.027,-0.011,-0.0041,0.019,-0.00086,-0.0057,-0.00012,0.069,-0.023,-0.14,-0.11,-0.024,0.5,0.084,-0.032,-0.07,0,0,0.00039,0.00038,0.046,0.022,0.022,0.0069,0.043,0.043,0.033,2.3e-06,2.1e-06,4.5e-06,0.03,0.03,0.00044,0.0012,6e-05,0.0012,0.0012,0.00072,0.0012,1,1 +16290000,-0.28,0.016,-0.0068,0.96,-0.028,-0.0011,0.027,-0.014,-0.0043,0.021,-0.00087,-0.0057,-0.00012,0.069,-0.023,-0.14,-0.11,-0.024,0.5,0.084,-0.032,-0.07,0,0,0.00039,0.00039,0.046,0.024,0.024,0.007,0.048,0.048,0.033,2.3e-06,2e-06,4.5e-06,0.03,0.03,0.00044,0.0012,6e-05,0.0012,0.0012,0.00072,0.0012,1,1 +16390000,-0.28,0.016,-0.0068,0.96,-0.028,-0.0015,0.027,-0.011,-0.0041,0.021,-0.00087,-0.0057,-0.00012,0.07,-0.023,-0.14,-0.11,-0.024,0.5,0.084,-0.032,-0.07,0,0,0.00039,0.00038,0.046,0.021,0.021,0.007,0.042,0.042,0.033,2.2e-06,1.9e-06,4.5e-06,0.03,0.03,0.00044,0.0012,6e-05,0.0012,0.0012,0.00072,0.0012,1,1 +16490000,-0.28,0.016,-0.0069,0.96,-0.033,-0.00051,0.029,-0.014,-0.0041,0.025,-0.00086,-0.0057,-0.00012,0.07,-0.023,-0.14,-0.11,-0.024,0.5,0.084,-0.032,-0.07,0,0,0.00039,0.00038,0.046,0.023,0.023,0.0072,0.047,0.047,0.033,2.2e-06,1.9e-06,4.5e-06,0.03,0.03,0.00044,0.0012,6e-05,0.0012,0.0012,0.00072,0.0012,1,1 +16590000,-0.28,0.016,-0.0069,0.96,-0.036,1.4e-05,0.033,-0.012,-0.0035,0.025,-0.00087,-0.0057,-0.00012,0.07,-0.023,-0.14,-0.11,-0.024,0.5,0.084,-0.032,-0.069,0,0,0.00039,0.00038,0.046,0.02,0.021,0.0072,0.042,0.042,0.033,2.1e-06,1.8e-06,4.5e-06,0.03,0.03,0.00043,0.0012,6e-05,0.0012,0.0012,0.00072,0.0012,1,1 +16690000,-0.28,0.016,-0.0069,0.96,-0.039,0.0036,0.033,-0.016,-0.0035,0.026,-0.00089,-0.0057,-0.00012,0.07,-0.023,-0.14,-0.11,-0.024,0.5,0.084,-0.032,-0.069,0,0,0.00039,0.00038,0.046,0.022,0.022,0.0073,0.047,0.047,0.033,2e-06,1.8e-06,4.5e-06,0.03,0.03,0.00043,0.0012,6e-05,0.0012,0.0012,0.00072,0.0012,1,1 +16790000,-0.28,0.016,-0.0068,0.96,-0.039,0.0034,0.032,-0.013,-0.0031,0.026,-0.00091,-0.0057,-0.00012,0.07,-0.023,-0.14,-0.11,-0.024,0.5,0.084,-0.032,-0.069,0,0,0.00039,0.00038,0.046,0.02,0.02,0.0073,0.042,0.042,0.033,2e-06,1.7e-06,4.5e-06,0.03,0.03,0.00043,0.0012,6e-05,0.0012,0.0012,0.00072,0.0012,1,1 +16890000,-0.28,0.016,-0.0067,0.96,-0.039,0.0029,0.033,-0.017,-0.0032,0.025,-0.00093,-0.0057,-0.00012,0.07,-0.023,-0.14,-0.11,-0.024,0.5,0.084,-0.032,-0.069,0,0,0.00039,0.00038,0.046,0.021,0.021,0.0074,0.046,0.046,0.033,1.9e-06,1.7e-06,4.5e-06,0.03,0.03,0.00042,0.0012,6e-05,0.0012,0.0012,0.00072,0.0012,1,1 +16990000,-0.28,0.016,-0.0067,0.96,-0.036,0.0033,0.033,-0.015,-0.0034,0.024,-0.00094,-0.0057,-0.00012,0.07,-0.023,-0.14,-0.11,-0.024,0.5,0.084,-0.032,-0.069,0,0,0.00039,0.00038,0.046,0.021,0.021,0.0074,0.049,0.049,0.033,1.9e-06,1.7e-06,4.5e-06,0.03,0.03,0.00042,0.0012,6e-05,0.0012,0.0012,0.00072,0.0012,1,1 +17090000,-0.28,0.016,-0.0068,0.96,-0.041,0.0053,0.033,-0.019,-0.0029,0.023,-0.00093,-0.0057,-0.00012,0.07,-0.023,-0.14,-0.11,-0.023,0.5,0.084,-0.032,-0.069,0,0,0.00039,0.00038,0.046,0.022,0.022,0.0075,0.054,0.054,0.033,1.9e-06,1.6e-06,4.5e-06,0.03,0.03,0.00042,0.0012,6e-05,0.0012,0.0012,0.00072,0.0012,1,1 +17190000,-0.28,0.016,-0.0069,0.96,-0.039,0.0071,0.034,-0.018,-0.0046,0.026,-0.00093,-0.0057,-0.00013,0.07,-0.023,-0.14,-0.11,-0.023,0.5,0.084,-0.033,-0.069,0,0,0.00039,0.00038,0.046,0.022,0.022,0.0076,0.057,0.057,0.033,1.8e-06,1.6e-06,4.5e-06,0.03,0.03,0.00041,0.0012,6e-05,0.0012,0.0012,0.00072,0.0012,1,1 +17290000,-0.28,0.016,-0.0069,0.96,-0.042,0.0077,0.034,-0.022,-0.0035,0.026,-0.00092,-0.0057,-0.00013,0.07,-0.023,-0.14,-0.11,-0.023,0.5,0.084,-0.033,-0.069,0,0,0.00039,0.00038,0.046,0.023,0.024,0.0077,0.062,0.063,0.033,1.8e-06,1.5e-06,4.5e-06,0.03,0.03,0.00041,0.0012,6e-05,0.0012,0.0012,0.00072,0.0012,1,1 +17390000,-0.28,0.016,-0.0069,0.96,-0.032,0.013,0.033,-0.014,-0.002,0.026,-0.00095,-0.0057,-0.00013,0.07,-0.023,-0.14,-0.11,-0.023,0.5,0.084,-0.033,-0.069,0,0,0.00039,0.00038,0.046,0.02,0.02,0.0076,0.052,0.052,0.033,1.7e-06,1.5e-06,4.5e-06,0.03,0.03,0.00041,0.0012,6e-05,0.0012,0.0012,0.00072,0.0012,1,1 +17490000,-0.28,0.016,-0.0069,0.96,-0.032,0.014,0.033,-0.017,-0.00051,0.028,-0.00094,-0.0057,-0.00013,0.07,-0.023,-0.14,-0.11,-0.023,0.5,0.084,-0.033,-0.069,0,0,0.00039,0.00038,0.046,0.021,0.022,0.0078,0.058,0.058,0.033,1.7e-06,1.5e-06,4.5e-06,0.03,0.03,0.0004,0.0012,6e-05,0.0012,0.0012,0.00072,0.0012,1,1 +17590000,-0.28,0.016,-0.0069,0.96,-0.032,0.012,0.032,-0.016,-0.00075,0.026,-0.00095,-0.0057,-0.00013,0.07,-0.023,-0.14,-0.11,-0.023,0.5,0.084,-0.033,-0.069,0,0,0.00038,0.00038,0.046,0.018,0.019,0.0077,0.049,0.049,0.033,1.6e-06,1.4e-06,4.4e-06,0.03,0.03,0.0004,0.0012,6e-05,0.0012,0.0012,0.00071,0.0012,1,1 +17690000,-0.28,0.016,-0.007,0.96,-0.033,0.012,0.033,-0.019,0.0003,0.028,-0.00096,-0.0057,-0.00013,0.07,-0.023,-0.14,-0.11,-0.023,0.5,0.084,-0.033,-0.069,0,0,0.00038,0.00038,0.046,0.02,0.02,0.0078,0.054,0.054,0.033,1.6e-06,1.4e-06,4.4e-06,0.03,0.03,0.00039,0.0012,6e-05,0.0012,0.0012,0.00071,0.0012,1,1 +17790000,-0.28,0.016,-0.007,0.96,-0.033,0.013,0.033,-0.018,0.0011,0.033,-0.00097,-0.0057,-0.00013,0.07,-0.023,-0.14,-0.11,-0.023,0.5,0.084,-0.033,-0.069,0,0,0.00038,0.00038,0.046,0.019,0.02,0.0078,0.057,0.057,0.033,1.6e-06,1.4e-06,4.4e-06,0.03,0.03,0.00039,0.0012,5.9e-05,0.0012,0.0012,0.00071,0.0012,1,1 +17890000,-0.28,0.016,-0.0069,0.96,-0.037,0.014,0.033,-0.021,0.0022,0.038,-0.00098,-0.0057,-0.00013,0.07,-0.023,-0.14,-0.11,-0.023,0.5,0.084,-0.033,-0.069,0,0,0.00038,0.00038,0.046,0.021,0.021,0.0079,0.062,0.062,0.033,1.5e-06,1.3e-06,4.4e-06,0.03,0.03,0.00039,0.0012,5.9e-05,0.0012,0.0012,0.00071,0.0012,1,1 +17990000,-0.28,0.016,-0.0069,0.96,-0.035,0.015,0.033,-0.017,0.0047,0.038,-0.00098,-0.0057,-0.00013,0.071,-0.023,-0.14,-0.11,-0.024,0.5,0.084,-0.033,-0.069,0,0,0.00038,0.00037,0.046,0.018,0.018,0.0079,0.052,0.052,0.033,1.5e-06,1.3e-06,4.4e-06,0.03,0.03,0.00038,0.0012,5.9e-05,0.0012,0.0012,0.00071,0.0012,1,1 +18090000,-0.28,0.016,-0.007,0.96,-0.037,0.016,0.032,-0.021,0.0061,0.037,-0.00098,-0.0057,-0.00013,0.071,-0.023,-0.14,-0.11,-0.024,0.5,0.084,-0.033,-0.069,0,0,0.00038,0.00037,0.046,0.019,0.02,0.008,0.057,0.057,0.034,1.5e-06,1.3e-06,4.4e-06,0.03,0.03,0.00038,0.0012,5.9e-05,0.0012,0.0012,0.00071,0.0012,1,1 +18190000,-0.28,0.016,-0.007,0.96,-0.034,0.013,0.033,-0.016,0.004,0.035,-0.001,-0.0057,-0.00013,0.071,-0.023,-0.14,-0.11,-0.024,0.5,0.084,-0.033,-0.069,0,0,0.00038,0.00037,0.046,0.017,0.017,0.0079,0.049,0.049,0.034,1.4e-06,1.2e-06,4.4e-06,0.03,0.03,0.00037,0.0012,5.9e-05,0.0012,0.0011,0.00071,0.0012,1,1 +18290000,-0.28,0.016,-0.007,0.96,-0.037,0.013,0.031,-0.02,0.005,0.033,-0.001,-0.0057,-0.00013,0.071,-0.023,-0.14,-0.11,-0.024,0.5,0.084,-0.033,-0.069,0,0,0.00038,0.00037,0.046,0.018,0.019,0.008,0.053,0.054,0.034,1.4e-06,1.2e-06,4.4e-06,0.03,0.03,0.00037,0.0012,5.9e-05,0.0012,0.0011,0.00071,0.0012,1,1 +18390000,-0.28,0.016,-0.0069,0.96,-0.033,0.013,0.031,-0.014,0.0042,0.033,-0.001,-0.0057,-0.00013,0.071,-0.023,-0.14,-0.11,-0.024,0.5,0.084,-0.033,-0.069,0,0,0.00038,0.00037,0.046,0.016,0.017,0.0079,0.046,0.047,0.034,1.4e-06,1.2e-06,4.4e-06,0.03,0.03,0.00036,0.0012,5.9e-05,0.0012,0.0011,0.00071,0.0012,1,1 +18490000,-0.28,0.016,-0.0069,0.96,-0.037,0.012,0.03,-0.018,0.005,0.034,-0.001,-0.0057,-0.00013,0.072,-0.022,-0.14,-0.11,-0.024,0.5,0.084,-0.033,-0.069,0,0,0.00038,0.00037,0.046,0.017,0.018,0.008,0.051,0.051,0.034,1.3e-06,1.2e-06,4.4e-06,0.03,0.03,0.00036,0.0012,5.9e-05,0.0012,0.0011,0.00071,0.0012,1,1 +18590000,-0.28,0.016,-0.0068,0.96,-0.035,0.012,0.03,-0.015,0.0045,0.037,-0.001,-0.0057,-0.00014,0.071,-0.022,-0.14,-0.11,-0.024,0.5,0.084,-0.033,-0.069,0,0,0.00038,0.00037,0.046,0.015,0.016,0.0079,0.044,0.045,0.034,1.3e-06,1.1e-06,4.4e-06,0.03,0.03,0.00035,0.0012,5.9e-05,0.0012,0.0011,0.00071,0.0012,1,1 +18690000,-0.28,0.016,-0.0067,0.96,-0.035,0.011,0.028,-0.017,0.0053,0.035,-0.0011,-0.0058,-0.00013,0.071,-0.022,-0.14,-0.11,-0.024,0.5,0.084,-0.033,-0.069,0,0,0.00038,0.00037,0.046,0.016,0.017,0.008,0.048,0.049,0.034,1.3e-06,1.1e-06,4.4e-06,0.03,0.03,0.00035,0.0012,5.9e-05,0.0012,0.0011,0.00071,0.0012,1,1 +18790000,-0.28,0.015,-0.0067,0.96,-0.031,0.011,0.028,-0.014,0.0046,0.033,-0.0011,-0.0058,-0.00014,0.071,-0.022,-0.14,-0.11,-0.024,0.5,0.084,-0.033,-0.069,0,0,0.00038,0.00037,0.046,0.015,0.016,0.0079,0.043,0.043,0.034,1.2e-06,1.1e-06,4.4e-06,0.03,0.03,0.00034,0.0012,5.9e-05,0.0012,0.0011,0.00071,0.0012,1,1 +18890000,-0.28,0.015,-0.0067,0.96,-0.032,0.011,0.026,-0.017,0.0057,0.029,-0.0011,-0.0058,-0.00014,0.071,-0.022,-0.14,-0.11,-0.024,0.5,0.084,-0.033,-0.069,0,0,0.00038,0.00037,0.046,0.016,0.017,0.008,0.047,0.047,0.034,1.2e-06,1.1e-06,4.4e-06,0.03,0.03,0.00034,0.0012,5.9e-05,0.0012,0.0011,0.00071,0.0012,1,1 +18990000,-0.28,0.015,-0.0066,0.96,-0.029,0.011,0.027,-0.015,0.005,0.033,-0.0011,-0.0058,-0.00014,0.071,-0.022,-0.14,-0.11,-0.023,0.5,0.084,-0.033,-0.069,0,0,0.00038,0.00037,0.046,0.015,0.015,0.0079,0.042,0.042,0.034,1.2e-06,1e-06,4.4e-06,0.03,0.03,0.00033,0.0012,5.9e-05,0.0012,0.0011,0.0007,0.0012,1,1 +19090000,-0.28,0.015,-0.0067,0.96,-0.029,0.012,0.027,-0.017,0.0056,0.029,-0.0011,-0.0058,-0.00014,0.071,-0.022,-0.14,-0.11,-0.023,0.5,0.084,-0.033,-0.069,0,0,0.00038,0.00037,0.046,0.016,0.016,0.0079,0.045,0.046,0.035,1.2e-06,1e-06,4.4e-06,0.03,0.03,0.00033,0.0012,5.9e-05,0.0012,0.0011,0.0007,0.0012,1,1 +19190000,-0.28,0.016,-0.0066,0.96,-0.026,0.013,0.027,-0.015,0.0056,0.028,-0.0011,-0.0058,-0.00015,0.071,-0.022,-0.14,-0.11,-0.023,0.5,0.084,-0.033,-0.069,0,0,0.00038,0.00037,0.046,0.014,0.015,0.0079,0.041,0.041,0.034,1.1e-06,9.8e-07,4.4e-06,0.03,0.03,0.00032,0.0012,5.9e-05,0.0012,0.0011,0.0007,0.0012,1,1 +19290000,-0.28,0.016,-0.0066,0.96,-0.027,0.013,0.027,-0.018,0.0067,0.027,-0.0011,-0.0058,-0.00015,0.071,-0.022,-0.14,-0.11,-0.023,0.5,0.084,-0.033,-0.069,0,0,0.00038,0.00037,0.046,0.015,0.016,0.0079,0.044,0.045,0.034,1.1e-06,9.7e-07,4.4e-06,0.03,0.03,0.00032,0.0012,5.9e-05,0.0012,0.0011,0.0007,0.0012,1,1 +19390000,-0.28,0.015,-0.0067,0.96,-0.025,0.011,0.028,-0.016,0.0067,0.026,-0.0011,-0.0058,-0.00015,0.071,-0.022,-0.14,-0.11,-0.023,0.5,0.084,-0.033,-0.069,0,0,0.00038,0.00037,0.046,0.014,0.015,0.0078,0.04,0.04,0.035,1.1e-06,9.4e-07,4.3e-06,0.03,0.03,0.00032,0.0012,5.9e-05,0.0012,0.0011,0.0007,0.0012,1,1 +19490000,-0.28,0.015,-0.0068,0.96,-0.028,0.012,0.028,-0.019,0.008,0.026,-0.0011,-0.0058,-0.00015,0.071,-0.022,-0.14,-0.11,-0.023,0.5,0.084,-0.033,-0.069,0,0,0.00038,0.00037,0.046,0.015,0.016,0.0078,0.043,0.044,0.035,1.1e-06,9.3e-07,4.3e-06,0.03,0.03,0.00031,0.0012,5.9e-05,0.0012,0.0011,0.0007,0.0012,1,1 +19590000,-0.28,0.015,-0.0067,0.96,-0.024,0.013,0.03,-0.016,0.0063,0.026,-0.0011,-0.0058,-0.00016,0.071,-0.022,-0.14,-0.11,-0.023,0.5,0.084,-0.033,-0.069,0,0,0.00038,0.00036,0.046,0.014,0.015,0.0077,0.039,0.04,0.035,1.1e-06,9e-07,4.3e-06,0.03,0.03,0.00031,0.0012,5.9e-05,0.0012,0.0011,0.0007,0.0012,1,1 +19690000,-0.28,0.016,-0.0067,0.96,-0.024,0.011,0.028,-0.019,0.007,0.026,-0.0011,-0.0058,-0.00016,0.072,-0.022,-0.13,-0.11,-0.023,0.5,0.084,-0.033,-0.069,0,0,0.00038,0.00037,0.046,0.015,0.016,0.0078,0.043,0.043,0.035,1e-06,8.9e-07,4.3e-06,0.03,0.03,0.0003,0.0012,5.9e-05,0.0012,0.0011,0.0007,0.0012,1,1 +19790000,-0.28,0.015,-0.0068,0.96,-0.021,0.011,0.026,-0.019,0.0075,0.022,-0.0011,-0.0058,-0.00016,0.072,-0.022,-0.13,-0.11,-0.023,0.5,0.084,-0.033,-0.069,0,0,0.00038,0.00037,0.046,0.015,0.016,0.0077,0.045,0.046,0.035,1e-06,8.7e-07,4.3e-06,0.03,0.03,0.0003,0.0012,5.9e-05,0.0012,0.0011,0.0007,0.0012,1,1 +19890000,-0.28,0.016,-0.0068,0.96,-0.023,0.011,0.027,-0.021,0.0087,0.02,-0.0011,-0.0058,-0.00016,0.072,-0.022,-0.13,-0.11,-0.023,0.5,0.084,-0.033,-0.069,0,0,0.00038,0.00037,0.046,0.016,0.017,0.0077,0.049,0.05,0.035,1e-06,8.6e-07,4.3e-06,0.03,0.03,0.00029,0.0012,5.9e-05,0.0012,0.0011,0.0007,0.0012,1,1 +19990000,-0.28,0.016,-0.0068,0.96,-0.02,0.012,0.024,-0.016,0.0081,0.017,-0.0011,-0.0058,-0.00016,0.072,-0.022,-0.13,-0.11,-0.023,0.5,0.084,-0.033,-0.069,0,0,0.00038,0.00036,0.046,0.014,0.015,0.0076,0.043,0.044,0.035,9.7e-07,8.3e-07,4.3e-06,0.03,0.03,0.00029,0.0012,5.8e-05,0.0012,0.0011,0.0007,0.0012,1,1 +20090000,-0.28,0.016,-0.0068,0.96,-0.022,0.015,0.024,-0.018,0.0093,0.02,-0.0011,-0.0058,-0.00016,0.072,-0.022,-0.13,-0.11,-0.023,0.5,0.084,-0.033,-0.069,0,0,0.00038,0.00036,0.046,0.015,0.017,0.0076,0.047,0.048,0.035,9.7e-07,8.2e-07,4.3e-06,0.03,0.03,0.00029,0.0012,5.8e-05,0.0012,0.0011,0.0007,0.0012,1,1 +20190000,-0.28,0.016,-0.0068,0.96,-0.023,0.013,0.025,-0.019,0.0097,0.02,-0.0011,-0.0058,-0.00017,0.072,-0.022,-0.13,-0.11,-0.023,0.5,0.084,-0.033,-0.069,0,0,0.00038,0.00036,0.046,0.015,0.017,0.0075,0.05,0.05,0.035,9.4e-07,8e-07,4.3e-06,0.03,0.03,0.00028,0.0012,5.8e-05,0.0012,0.0011,0.0007,0.0012,1,1 +20290000,-0.28,0.016,-0.0068,0.96,-0.021,0.015,0.025,-0.022,0.011,0.021,-0.0011,-0.0058,-0.00017,0.072,-0.022,-0.13,-0.11,-0.023,0.5,0.084,-0.033,-0.069,0,0,0.00038,0.00036,0.046,0.016,0.018,0.0075,0.054,0.055,0.035,9.3e-07,7.9e-07,4.3e-06,0.03,0.03,0.00028,0.0012,5.8e-05,0.0012,0.0011,0.0007,0.0012,1,1 +20390000,-0.28,0.016,-0.0067,0.96,-0.019,0.013,0.025,-0.022,0.01,0.022,-0.0011,-0.0058,-0.00017,0.072,-0.022,-0.13,-0.11,-0.023,0.5,0.084,-0.034,-0.069,0,0,0.00038,0.00036,0.046,0.016,0.018,0.0075,0.056,0.057,0.035,9.1e-07,7.7e-07,4.3e-06,0.03,0.03,0.00027,0.0012,5.8e-05,0.0012,0.0011,0.0007,0.0012,1,1 +20490000,-0.28,0.016,-0.0067,0.96,-0.017,0.015,0.025,-0.024,0.011,0.02,-0.0011,-0.0058,-0.00017,0.072,-0.022,-0.13,-0.11,-0.023,0.5,0.084,-0.033,-0.069,0,0,0.00038,0.00036,0.046,0.017,0.019,0.0075,0.061,0.063,0.035,9e-07,7.7e-07,4.3e-06,0.03,0.03,0.00027,0.0012,5.8e-05,0.0012,0.0011,0.0007,0.0012,1,1 +20590000,-0.28,0.016,-0.0066,0.96,-0.017,0.014,0.025,-0.024,0.01,0.018,-0.0011,-0.0058,-0.00017,0.072,-0.022,-0.13,-0.11,-0.023,0.5,0.084,-0.033,-0.069,0,0,0.00038,0.00036,0.046,0.017,0.019,0.0074,0.064,0.065,0.035,8.8e-07,7.5e-07,4.2e-06,0.03,0.03,0.00026,0.0012,5.8e-05,0.0012,0.0011,0.00069,0.0012,1,1 +20690000,-0.28,0.016,-0.0066,0.96,-0.016,0.014,0.026,-0.025,0.012,0.019,-0.0011,-0.0058,-0.00017,0.072,-0.022,-0.13,-0.11,-0.023,0.5,0.084,-0.034,-0.069,0,0,0.00038,0.00036,0.046,0.018,0.02,0.0074,0.069,0.071,0.035,8.7e-07,7.4e-07,4.2e-06,0.03,0.03,0.00026,0.0012,5.8e-05,0.0012,0.0011,0.00069,0.0012,1,1 +20790000,-0.28,0.016,-0.006,0.96,-0.01,0.01,0.01,-0.019,0.0088,0.017,-0.0012,-0.0058,-0.00018,0.072,-0.021,-0.13,-0.11,-0.023,0.5,0.084,-0.034,-0.069,0,0,0.00038,0.00036,0.046,0.016,0.017,0.0073,0.057,0.057,0.035,8.4e-07,7.1e-07,4.2e-06,0.03,0.03,0.00026,0.0012,5.8e-05,0.0012,0.0011,0.00069,0.0012,1,1 +20890000,-0.28,0.011,0.0027,0.96,-0.0058,-5.9e-05,-0.11,-0.021,0.0092,0.011,-0.0012,-0.0058,-0.00018,0.072,-0.022,-0.13,-0.11,-0.023,0.5,0.084,-0.033,-0.069,0,0,0.00037,0.00034,0.046,0.017,0.018,0.0073,0.061,0.062,0.035,8.3e-07,7.1e-07,4.2e-06,0.03,0.03,0.00025,0.0012,5.8e-05,0.0012,0.0011,0.00069,0.0012,1,1 +20990000,-0.28,0.0071,0.0057,0.96,0.0086,-0.017,-0.25,-0.017,0.007,-0.004,-0.0011,-0.0058,-0.00019,0.072,-0.022,-0.13,-0.11,-0.024,0.5,0.084,-0.033,-0.068,0,0,0.00036,0.00034,0.046,0.015,0.016,0.0072,0.052,0.052,0.034,8e-07,6.9e-07,4.2e-06,0.03,0.03,0.00025,0.0012,5.8e-05,0.0012,0.0011,0.00069,0.0012,1,1 +21090000,-0.28,0.0076,0.0042,0.96,0.022,-0.029,-0.37,-0.016,0.005,-0.035,-0.0011,-0.0058,-0.00019,0.072,-0.022,-0.13,-0.11,-0.024,0.5,0.084,-0.033,-0.068,0,0,0.00036,0.00034,0.046,0.016,0.017,0.0072,0.056,0.057,0.035,8e-07,6.8e-07,4.2e-06,0.03,0.03,0.00025,0.0011,5.8e-05,0.0012,0.0011,0.00069,0.0012,1,1 +21190000,-0.28,0.0095,0.0015,0.96,0.028,-0.035,-0.49,-0.013,0.0038,-0.071,-0.0011,-0.0058,-0.00018,0.072,-0.022,-0.13,-0.11,-0.024,0.5,0.084,-0.033,-0.068,0,0,0.00036,0.00034,0.046,0.014,0.016,0.0071,0.048,0.049,0.035,7.7e-07,6.6e-07,4.1e-06,0.03,0.03,0.00024,0.0011,5.8e-05,0.0012,0.0011,0.00068,0.0012,1,1 +21290000,-0.28,0.011,-0.00054,0.96,0.026,-0.037,-0.62,-0.011,0.0011,-0.13,-0.0011,-0.0058,-0.00019,0.072,-0.023,-0.13,-0.11,-0.024,0.5,0.084,-0.033,-0.068,0,0,0.00036,0.00034,0.046,0.015,0.017,0.0071,0.052,0.053,0.035,7.7e-07,6.5e-07,4.1e-06,0.03,0.03,0.00024,0.0011,5.8e-05,0.0012,0.0011,0.00068,0.0012,1,1 +21390000,-0.28,0.012,-0.002,0.96,0.019,-0.029,-0.75,-0.013,0.0046,-0.19,-0.0011,-0.0058,-0.00017,0.072,-0.024,-0.13,-0.11,-0.024,0.5,0.084,-0.033,-0.068,0,0,0.00037,0.00034,0.046,0.016,0.017,0.007,0.054,0.055,0.035,7.5e-07,6.4e-07,4.1e-06,0.03,0.03,0.00024,0.0011,5.8e-05,0.0012,0.0011,0.00068,0.0012,1,1 +21490000,-0.28,0.012,-0.0028,0.96,0.013,-0.027,-0.88,-0.011,0.0015,-0.28,-0.0011,-0.0058,-0.00017,0.072,-0.024,-0.13,-0.11,-0.024,0.5,0.084,-0.033,-0.068,0,0,0.00037,0.00034,0.046,0.017,0.018,0.007,0.059,0.06,0.035,7.4e-07,6.3e-07,4.1e-06,0.03,0.03,0.00023,0.0011,5.7e-05,0.0012,0.0011,0.00068,0.0012,1,1 +21590000,-0.28,0.012,-0.0034,0.96,0.0009,-0.021,-1,-0.015,0.0065,-0.37,-0.0011,-0.0058,-0.00014,0.072,-0.025,-0.13,-0.11,-0.024,0.5,0.083,-0.033,-0.068,0,0,0.00036,0.00034,0.045,0.017,0.018,0.0069,0.061,0.063,0.034,7.3e-07,6.2e-07,4.1e-06,0.03,0.029,0.00023,0.0011,5.7e-05,0.0012,0.0011,0.00068,0.0012,1,1 +21690000,-0.28,0.012,-0.0037,0.96,-0.0046,-0.017,-1.1,-0.015,0.004,-0.48,-0.0011,-0.0058,-0.00014,0.072,-0.025,-0.13,-0.11,-0.024,0.5,0.083,-0.033,-0.068,0,0,0.00036,0.00034,0.045,0.018,0.02,0.0069,0.066,0.068,0.035,7.2e-07,6.1e-07,4.1e-06,0.03,0.029,0.00023,0.0011,5.7e-05,0.0012,0.0011,0.00068,0.0012,1,1 +21790000,-0.28,0.012,-0.0041,0.96,-0.011,-0.0087,-1.3,-0.017,0.01,-0.6,-0.0011,-0.0058,-0.00011,0.072,-0.026,-0.13,-0.11,-0.024,0.5,0.083,-0.032,-0.068,0,0,0.00036,0.00034,0.045,0.018,0.019,0.0069,0.069,0.07,0.034,7e-07,6e-07,4.1e-06,0.03,0.029,0.00022,0.0011,5.7e-05,0.0012,0.0011,0.00068,0.0012,1,1 +21890000,-0.28,0.013,-0.0044,0.96,-0.017,-0.0041,-1.4,-0.018,0.01,-0.74,-0.0011,-0.0058,-0.00011,0.072,-0.026,-0.13,-0.11,-0.024,0.5,0.083,-0.032,-0.068,0,0,0.00036,0.00034,0.045,0.019,0.021,0.0068,0.074,0.076,0.034,7e-07,6e-07,4.1e-06,0.03,0.029,0.00022,0.0011,5.7e-05,0.0012,0.0011,0.00068,0.0012,1,1 +21990000,-0.28,0.013,-0.0052,0.96,-0.022,0.0043,-1.4,-0.024,0.018,-0.88,-0.0011,-0.0058,-8.8e-05,0.072,-0.027,-0.13,-0.11,-0.024,0.5,0.083,-0.032,-0.068,0,0,0.00036,0.00034,0.045,0.018,0.02,0.0068,0.077,0.079,0.034,6.8e-07,5.8e-07,4e-06,0.03,0.029,0.00022,0.0011,5.7e-05,0.0012,0.0011,0.00068,0.0012,1,1 +22090000,-0.28,0.014,-0.0058,0.96,-0.025,0.0075,-1.4,-0.025,0.018,-1,-0.0011,-0.0058,-8.2e-05,0.072,-0.027,-0.13,-0.11,-0.024,0.5,0.083,-0.032,-0.068,0,0,0.00036,0.00035,0.045,0.019,0.021,0.0068,0.083,0.085,0.034,6.8e-07,5.8e-07,4e-06,0.03,0.029,0.00022,0.0011,5.7e-05,0.0012,0.0011,0.00068,0.0012,1,1 +22190000,-0.28,0.014,-0.0063,0.96,-0.032,0.014,-1.4,-0.03,0.025,-1.2,-0.0011,-0.0058,-5.9e-05,0.072,-0.028,-0.13,-0.11,-0.024,0.5,0.083,-0.032,-0.068,0,0,0.00036,0.00035,0.045,0.019,0.021,0.0067,0.085,0.087,0.034,6.6e-07,5.6e-07,4e-06,0.029,0.029,0.00021,0.0011,5.7e-05,0.0012,0.0011,0.00067,0.0012,1,1 +22290000,-0.28,0.014,-0.007,0.96,-0.04,0.019,-1.4,-0.034,0.027,-1.3,-0.0011,-0.0058,-6e-05,0.072,-0.028,-0.13,-0.11,-0.024,0.5,0.083,-0.032,-0.068,0,0,0.00036,0.00035,0.045,0.02,0.022,0.0067,0.091,0.094,0.034,6.6e-07,5.6e-07,4e-06,0.029,0.029,0.00021,0.0011,5.7e-05,0.0012,0.0011,0.00067,0.0012,1,1 +22390000,-0.28,0.015,-0.0073,0.96,-0.047,0.026,-1.4,-0.04,0.031,-1.5,-0.0011,-0.0058,-6.1e-05,0.072,-0.028,-0.13,-0.11,-0.024,0.5,0.083,-0.032,-0.068,0,0,0.00036,0.00035,0.045,0.019,0.021,0.0066,0.094,0.096,0.034,6.4e-07,5.5e-07,4e-06,0.029,0.029,0.00021,0.0011,5.7e-05,0.0012,0.0011,0.00067,0.0012,1,1 +22490000,-0.28,0.015,-0.0074,0.96,-0.053,0.032,-1.4,-0.045,0.034,-1.6,-0.0011,-0.0058,-6.4e-05,0.072,-0.028,-0.13,-0.11,-0.024,0.5,0.083,-0.032,-0.068,0,0,0.00037,0.00035,0.045,0.02,0.022,0.0066,0.1,0.1,0.034,6.4e-07,5.4e-07,4e-06,0.029,0.029,0.00021,0.0011,5.7e-05,0.0012,0.0011,0.00067,0.0012,1,1 +22590000,-0.28,0.016,-0.0073,0.96,-0.058,0.038,-1.4,-0.046,0.038,-1.7,-0.0011,-0.0058,-5.7e-05,0.072,-0.028,-0.13,-0.11,-0.024,0.5,0.083,-0.032,-0.068,0,0,0.00037,0.00035,0.045,0.02,0.022,0.0065,0.1,0.11,0.034,6.2e-07,5.3e-07,3.9e-06,0.029,0.029,0.0002,0.0011,5.7e-05,0.0012,0.0011,0.00067,0.0012,1,1 +22690000,-0.28,0.016,-0.0072,0.96,-0.062,0.042,-1.4,-0.053,0.042,-1.9,-0.0011,-0.0058,-5.8e-05,0.072,-0.028,-0.13,-0.11,-0.024,0.5,0.083,-0.032,-0.068,0,0,0.00037,0.00035,0.045,0.021,0.023,0.0065,0.11,0.11,0.034,6.2e-07,5.3e-07,3.9e-06,0.029,0.029,0.0002,0.0011,5.7e-05,0.0012,0.0011,0.00067,0.0012,1,1 +22790000,-0.28,0.016,-0.0071,0.96,-0.069,0.047,-1.4,-0.059,0.045,-2,-0.0011,-0.0058,-6.6e-05,0.072,-0.028,-0.13,-0.11,-0.024,0.5,0.083,-0.032,-0.068,0,0,0.00037,0.00035,0.045,0.02,0.022,0.0065,0.11,0.11,0.034,6e-07,5.1e-07,3.9e-06,0.029,0.029,0.0002,0.0011,5.7e-05,0.0012,0.0011,0.00067,0.0012,1,1 +22890000,-0.28,0.017,-0.0072,0.96,-0.074,0.051,-1.4,-0.065,0.048,-2.2,-0.0011,-0.0058,-5.8e-05,0.072,-0.028,-0.13,-0.11,-0.024,0.5,0.083,-0.032,-0.068,0,0,0.00037,0.00035,0.045,0.021,0.023,0.0065,0.12,0.12,0.034,6e-07,5.1e-07,3.9e-06,0.029,0.029,0.0002,0.0011,5.7e-05,0.0012,0.0011,0.00067,0.0012,1,1 +22990000,-0.28,0.017,-0.007,0.96,-0.076,0.051,-1.4,-0.068,0.047,-2.3,-0.0011,-0.0058,-6.3e-05,0.072,-0.027,-0.13,-0.11,-0.024,0.5,0.083,-0.032,-0.068,0,0,0.00037,0.00034,0.045,0.02,0.022,0.0064,0.12,0.12,0.034,5.8e-07,5e-07,3.9e-06,0.029,0.029,0.00019,0.0011,5.7e-05,0.0012,0.0011,0.00067,0.0012,1,1 +23090000,-0.28,0.017,-0.007,0.96,-0.082,0.055,-1.4,-0.075,0.053,-2.5,-0.0011,-0.0058,-6.1e-05,0.072,-0.027,-0.13,-0.11,-0.024,0.5,0.083,-0.032,-0.068,0,0,0.00037,0.00035,0.045,0.021,0.023,0.0064,0.13,0.13,0.034,5.8e-07,5e-07,3.9e-06,0.029,0.029,0.00019,0.0011,5.7e-05,0.0012,0.0011,0.00067,0.0012,1,1 +23190000,-0.28,0.017,-0.0069,0.96,-0.084,0.05,-1.4,-0.074,0.049,-2.6,-0.0011,-0.0058,-8.4e-05,0.072,-0.026,-0.13,-0.11,-0.024,0.5,0.083,-0.032,-0.068,0,0,0.00037,0.00034,0.045,0.021,0.023,0.0063,0.13,0.13,0.033,5.7e-07,4.9e-07,3.8e-06,0.029,0.029,0.00019,0.0011,5.7e-05,0.0012,0.0011,0.00067,0.0012,1,1 +23290000,-0.28,0.018,-0.0073,0.96,-0.09,0.054,-1.4,-0.083,0.053,-2.7,-0.0011,-0.0058,-8e-05,0.072,-0.026,-0.13,-0.11,-0.024,0.5,0.083,-0.032,-0.068,0,0,0.00037,0.00035,0.045,0.022,0.024,0.0063,0.14,0.14,0.034,5.6e-07,4.8e-07,3.8e-06,0.029,0.029,0.00019,0.0011,5.7e-05,0.0012,0.0011,0.00067,0.0012,1,1 +23390000,-0.28,0.018,-0.0073,0.96,-0.089,0.056,-1.4,-0.077,0.055,-2.9,-0.0011,-0.0058,-9.8e-05,0.071,-0.026,-0.13,-0.11,-0.024,0.5,0.084,-0.032,-0.068,0,0,0.00037,0.00034,0.045,0.021,0.023,0.0063,0.14,0.14,0.033,5.5e-07,4.7e-07,3.8e-06,0.029,0.029,0.00018,0.0011,5.7e-05,0.0012,0.0011,0.00067,0.0012,1,1 +23490000,-0.28,0.018,-0.0073,0.96,-0.096,0.057,-1.4,-0.088,0.059,-3,-0.0011,-0.0058,-9e-05,0.071,-0.026,-0.13,-0.11,-0.024,0.5,0.084,-0.032,-0.068,0,0,0.00037,0.00035,0.045,0.022,0.024,0.0063,0.15,0.15,0.033,5.5e-07,4.7e-07,3.8e-06,0.029,0.029,0.00018,0.0011,5.7e-05,0.0012,0.0011,0.00067,0.0012,1,1 +23590000,-0.28,0.018,-0.0074,0.96,-0.094,0.051,-1.4,-0.083,0.049,-3.2,-0.0011,-0.0058,-0.00011,0.071,-0.025,-0.13,-0.11,-0.024,0.5,0.084,-0.033,-0.068,0,0,0.00037,0.00034,0.045,0.021,0.023,0.0062,0.15,0.15,0.033,5.4e-07,4.6e-07,3.8e-06,0.029,0.029,0.00018,0.0011,5.7e-05,0.0012,0.0011,0.00067,0.0012,1,1 +23690000,-0.28,0.019,-0.008,0.96,-0.093,0.053,-1.3,-0.092,0.053,-3.3,-0.0011,-0.0058,-0.00011,0.071,-0.025,-0.13,-0.11,-0.024,0.5,0.084,-0.033,-0.068,0,0,0.00037,0.00035,0.045,0.022,0.024,0.0062,0.16,0.16,0.033,5.4e-07,4.6e-07,3.8e-06,0.029,0.029,0.00018,0.0011,5.7e-05,0.0012,0.0011,0.00067,0.0012,1,1 +23790000,-0.28,0.021,-0.0095,0.96,-0.077,0.049,-0.95,-0.082,0.049,-3.4,-0.0012,-0.0058,-0.00012,0.071,-0.025,-0.13,-0.11,-0.024,0.5,0.084,-0.033,-0.068,0,0,0.00039,0.00036,0.045,0.021,0.022,0.0061,0.16,0.16,0.033,5.2e-07,4.5e-07,3.7e-06,0.029,0.029,0.00018,0.0011,5.7e-05,0.0012,0.0011,0.00067,0.0012,1,1 +23890000,-0.28,0.026,-0.012,0.96,-0.071,0.05,-0.52,-0.089,0.053,-3.5,-0.0012,-0.0058,-0.00011,0.071,-0.025,-0.13,-0.11,-0.024,0.5,0.083,-0.033,-0.067,0,0,0.00042,0.00038,0.045,0.021,0.023,0.0061,0.17,0.17,0.033,5.2e-07,4.5e-07,3.7e-06,0.029,0.029,0.00018,0.0011,5.7e-05,0.0012,0.0011,0.00067,0.0012,1,1 +23990000,-0.28,0.028,-0.014,0.96,-0.061,0.048,-0.13,-0.077,0.048,-3.6,-0.0012,-0.0058,-0.00013,0.071,-0.025,-0.13,-0.11,-0.024,0.5,0.083,-0.033,-0.067,0,0,0.00043,0.0004,0.045,0.02,0.022,0.0061,0.17,0.17,0.033,5.1e-07,4.4e-07,3.7e-06,0.029,0.029,0.00017,0.0011,5.7e-05,0.0012,0.0011,0.00067,0.0012,1,1 +24090000,-0.28,0.027,-0.014,0.96,-0.067,0.055,0.1,-0.082,0.053,-3.6,-0.0012,-0.0058,-0.00013,0.071,-0.025,-0.13,-0.11,-0.024,0.5,0.083,-0.033,-0.067,0,0,0.00043,0.0004,0.045,0.021,0.023,0.0061,0.18,0.18,0.033,5.1e-07,4.4e-07,3.7e-06,0.029,0.029,0.00017,0.0011,5.7e-05,0.0012,0.0011,0.00067,0.0012,1,1 +24190000,-0.28,0.023,-0.011,0.96,-0.071,0.052,0.089,-0.069,0.04,-3.6,-0.0012,-0.0058,-0.00015,0.072,-0.026,-0.13,-0.11,-0.024,0.5,0.083,-0.033,-0.066,0,0,0.0004,0.00037,0.045,0.02,0.022,0.006,0.18,0.18,0.033,5e-07,4.3e-07,3.6e-06,0.029,0.029,0.00017,0.0011,5.7e-05,0.0012,0.0011,0.00067,0.0012,1,1 +24290000,-0.28,0.02,-0.0093,0.96,-0.076,0.055,0.068,-0.076,0.045,-3.6,-0.0012,-0.0058,-0.00015,0.072,-0.026,-0.13,-0.11,-0.024,0.5,0.083,-0.033,-0.066,0,0,0.00038,0.00036,0.045,0.021,0.023,0.006,0.19,0.19,0.033,5e-07,4.3e-07,3.6e-06,0.029,0.029,0.00017,0.0011,5.7e-05,0.0012,0.0011,0.00067,0.0012,1,1 +24390000,-0.28,0.018,-0.0085,0.96,-0.059,0.048,0.084,-0.058,0.036,-3.6,-0.0013,-0.0058,-0.00016,0.073,-0.027,-0.13,-0.11,-0.024,0.5,0.083,-0.033,-0.066,0,0,0.00038,0.00035,0.045,0.02,0.022,0.006,0.19,0.19,0.033,4.9e-07,4.2e-07,3.6e-06,0.029,0.029,0.00017,0.0011,5.6e-05,0.0012,0.0011,0.00067,0.0012,1,1 +24490000,-0.28,0.019,-0.0087,0.96,-0.054,0.044,0.081,-0.063,0.039,-3.6,-0.0013,-0.0058,-0.00014,0.073,-0.027,-0.13,-0.11,-0.024,0.5,0.083,-0.033,-0.066,0,0,0.00038,0.00035,0.045,0.021,0.023,0.006,0.2,0.2,0.033,4.9e-07,4.2e-07,3.6e-06,0.029,0.029,0.00017,0.0011,5.6e-05,0.0012,0.0011,0.00067,0.0012,1,1 +24590000,-0.28,0.019,-0.0094,0.96,-0.043,0.042,0.077,-0.045,0.033,-3.6,-0.0013,-0.0058,-0.00016,0.074,-0.028,-0.13,-0.11,-0.024,0.5,0.083,-0.033,-0.066,0,0,0.00038,0.00036,0.045,0.021,0.022,0.0059,0.2,0.2,0.033,4.8e-07,4.1e-07,3.6e-06,0.029,0.029,0.00016,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 +24690000,-0.28,0.019,-0.0099,0.96,-0.041,0.041,0.076,-0.049,0.037,-3.5,-0.0013,-0.0058,-0.00016,0.074,-0.028,-0.13,-0.11,-0.024,0.5,0.083,-0.033,-0.066,0,0,0.00038,0.00036,0.045,0.021,0.024,0.0059,0.21,0.21,0.033,4.8e-07,4.1e-07,3.6e-06,0.029,0.029,0.00016,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 +24790000,-0.28,0.018,-0.01,0.96,-0.034,0.04,0.068,-0.037,0.029,-3.5,-0.0013,-0.0058,-0.00017,0.074,-0.029,-0.13,-0.11,-0.024,0.5,0.083,-0.033,-0.066,0,0,0.00037,0.00036,0.045,0.021,0.023,0.0059,0.21,0.21,0.032,4.7e-07,4.1e-07,3.5e-06,0.029,0.029,0.00016,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 +24890000,-0.28,0.017,-0.0099,0.96,-0.033,0.042,0.058,-0.04,0.032,-3.5,-0.0013,-0.0058,-0.00016,0.074,-0.029,-0.13,-0.11,-0.024,0.5,0.083,-0.033,-0.066,0,0,0.00037,0.00036,0.045,0.022,0.024,0.0059,0.22,0.22,0.032,4.7e-07,4e-07,3.5e-06,0.029,0.029,0.00016,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 +24990000,-0.28,0.017,-0.0097,0.96,-0.021,0.043,0.05,-0.026,0.027,-3.5,-0.0013,-0.0058,-0.00018,0.074,-0.03,-0.13,-0.11,-0.024,0.5,0.083,-0.033,-0.066,0,0,0.00037,0.00036,0.045,0.021,0.023,0.0058,0.22,0.22,0.032,4.6e-07,4e-07,3.5e-06,0.029,0.029,0.00016,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 +25090000,-0.28,0.017,-0.01,0.96,-0.016,0.043,0.048,-0.027,0.031,-3.5,-0.0013,-0.0058,-0.00018,0.074,-0.03,-0.13,-0.11,-0.024,0.5,0.083,-0.033,-0.066,0,0,0.00037,0.00036,0.045,0.022,0.024,0.0058,0.23,0.23,0.032,4.6e-07,4e-07,3.5e-06,0.029,0.029,0.00016,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 +25190000,-0.28,0.017,-0.01,0.96,-0.006,0.038,0.048,-0.011,0.021,-3.5,-0.0013,-0.0059,-0.0002,0.075,-0.03,-0.13,-0.11,-0.024,0.5,0.083,-0.033,-0.066,0,0,0.00037,0.00036,0.045,0.021,0.023,0.0058,0.23,0.23,0.032,4.5e-07,3.9e-07,3.5e-06,0.029,0.029,0.00016,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 +25290000,-0.28,0.016,-0.01,0.96,-0.0013,0.041,0.043,-0.012,0.026,-3.5,-0.0013,-0.0059,-0.00021,0.075,-0.03,-0.13,-0.11,-0.024,0.5,0.083,-0.033,-0.066,0,0,0.00037,0.00036,0.045,0.022,0.024,0.0058,0.24,0.24,0.032,4.5e-07,3.9e-07,3.5e-06,0.029,0.029,0.00015,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 +25390000,-0.28,0.016,-0.011,0.96,0.0076,0.039,0.041,-0.0022,0.02,-3.5,-0.0013,-0.0059,-0.00022,0.075,-0.031,-0.13,-0.11,-0.024,0.5,0.083,-0.033,-0.066,0,0,0.00036,0.00036,0.045,0.021,0.023,0.0058,0.24,0.24,0.032,4.4e-07,3.8e-07,3.4e-06,0.029,0.029,0.00015,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 +25490000,-0.28,0.016,-0.011,0.96,0.012,0.04,0.041,-0.0021,0.024,-3.5,-0.0013,-0.0059,-0.00022,0.075,-0.031,-0.13,-0.11,-0.024,0.5,0.083,-0.033,-0.066,0,0,0.00036,0.00036,0.045,0.022,0.024,0.0058,0.25,0.25,0.032,4.4e-07,3.8e-07,3.4e-06,0.029,0.029,0.00015,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 +25590000,-0.28,0.016,-0.011,0.96,0.017,0.035,0.042,0.0049,0.0094,-3.5,-0.0014,-0.0058,-0.00024,0.075,-0.031,-0.13,-0.11,-0.024,0.5,0.083,-0.033,-0.066,0,0,0.00036,0.00036,0.045,0.021,0.023,0.0057,0.25,0.25,0.032,4.3e-07,3.8e-07,3.4e-06,0.029,0.029,0.00015,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 +25690000,-0.28,0.015,-0.01,0.96,0.018,0.034,0.031,0.0066,0.013,-3.5,-0.0014,-0.0058,-0.00024,0.075,-0.031,-0.13,-0.11,-0.024,0.5,0.083,-0.033,-0.066,0,0,0.00036,0.00036,0.045,0.022,0.024,0.0057,0.26,0.26,0.032,4.3e-07,3.8e-07,3.4e-06,0.029,0.029,0.00015,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 +25790000,-0.28,0.015,-0.01,0.96,0.028,0.029,0.031,0.014,0.0031,-3.5,-0.0014,-0.0058,-0.00025,0.075,-0.031,-0.13,-0.11,-0.024,0.5,0.083,-0.033,-0.066,0,0,0.00036,0.00036,0.045,0.021,0.023,0.0057,0.26,0.26,0.032,4.2e-07,3.7e-07,3.4e-06,0.029,0.029,0.00015,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 +25890000,-0.28,0.015,-0.01,0.96,0.034,0.029,0.033,0.017,0.0067,-3.5,-0.0014,-0.0058,-0.00026,0.075,-0.031,-0.13,-0.11,-0.024,0.5,0.083,-0.033,-0.066,0,0,0.00036,0.00036,0.045,0.022,0.024,0.0057,0.27,0.27,0.032,4.2e-07,3.7e-07,3.3e-06,0.029,0.029,0.00015,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 +25990000,-0.28,0.015,-0.01,0.96,0.036,0.024,0.027,0.013,-0.004,-3.5,-0.0014,-0.0058,-0.00028,0.075,-0.031,-0.13,-0.11,-0.024,0.5,0.083,-0.033,-0.066,0,0,0.00036,0.00036,0.045,0.021,0.023,0.0057,0.27,0.27,0.032,4.2e-07,3.6e-07,3.3e-06,0.029,0.029,0.00015,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 +26090000,-0.28,0.015,-0.0099,0.96,0.041,0.024,0.025,0.017,-0.0023,-3.5,-0.0014,-0.0058,-0.00027,0.075,-0.031,-0.13,-0.11,-0.024,0.5,0.083,-0.033,-0.066,0,0,0.00036,0.00036,0.045,0.022,0.024,0.0057,0.28,0.28,0.032,4.2e-07,3.6e-07,3.3e-06,0.029,0.029,0.00015,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 +26190000,-0.28,0.015,-0.0098,0.96,0.046,0.015,0.021,0.02,-0.018,-3.5,-0.0014,-0.0058,-0.00028,0.075,-0.032,-0.13,-0.11,-0.024,0.5,0.083,-0.033,-0.066,0,0,0.00036,0.00036,0.045,0.021,0.023,0.0056,0.27,0.28,0.032,4.1e-07,3.6e-07,3.3e-06,0.029,0.029,0.00014,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 +26290000,-0.28,0.016,-0.0098,0.96,0.046,0.014,0.015,0.024,-0.016,-3.5,-0.0014,-0.0058,-0.00029,0.075,-0.032,-0.13,-0.11,-0.024,0.5,0.083,-0.033,-0.066,0,0,0.00037,0.00036,0.045,0.022,0.024,0.0056,0.29,0.29,0.032,4.1e-07,3.6e-07,3.3e-06,0.029,0.029,0.00014,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 +26390000,-0.28,0.016,-0.0092,0.96,0.043,0.0051,0.019,0.015,-0.03,-3.5,-0.0014,-0.0058,-0.0003,0.075,-0.032,-0.13,-0.11,-0.024,0.5,0.083,-0.033,-0.066,0,0,0.00037,0.00036,0.045,0.021,0.023,0.0056,0.28,0.29,0.032,4e-07,3.5e-07,3.3e-06,0.029,0.029,0.00014,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 +26490000,-0.28,0.016,-0.009,0.96,0.046,0.0028,0.028,0.02,-0.03,-3.5,-0.0014,-0.0058,-0.00031,0.075,-0.032,-0.13,-0.11,-0.024,0.5,0.083,-0.033,-0.066,0,0,0.00037,0.00036,0.045,0.022,0.024,0.0056,0.3,0.3,0.032,4e-07,3.5e-07,3.2e-06,0.029,0.029,0.00014,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 +26590000,-0.28,0.016,-0.0084,0.96,0.045,-0.0071,0.029,0.019,-0.042,-3.5,-0.0014,-0.0058,-0.00032,0.075,-0.032,-0.13,-0.11,-0.024,0.5,0.083,-0.034,-0.066,0,0,0.00037,0.00035,0.045,0.021,0.023,0.0056,0.29,0.3,0.032,4e-07,3.5e-07,3.2e-06,0.029,0.029,0.00014,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 +26690000,-0.28,0.016,-0.0083,0.96,0.047,-0.011,0.027,0.023,-0.042,-3.5,-0.0014,-0.0058,-0.00033,0.075,-0.032,-0.13,-0.11,-0.024,0.5,0.083,-0.034,-0.066,0,0,0.00037,0.00035,0.045,0.022,0.024,0.0056,0.31,0.31,0.032,4e-07,3.5e-07,3.2e-06,0.029,0.029,0.00014,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 +26790000,-0.27,0.015,-0.0081,0.96,0.049,-0.017,0.027,0.02,-0.055,-3.5,-0.0014,-0.0058,-0.00035,0.075,-0.032,-0.13,-0.11,-0.024,0.5,0.083,-0.034,-0.066,0,0,0.00037,0.00035,0.045,0.021,0.023,0.0055,0.3,0.31,0.031,3.9e-07,3.4e-07,3.2e-06,0.029,0.029,0.00014,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 +26890000,-0.27,0.015,-0.0074,0.96,0.055,-0.02,0.022,0.026,-0.057,-3.5,-0.0014,-0.0058,-0.00034,0.075,-0.032,-0.13,-0.11,-0.024,0.5,0.083,-0.034,-0.066,0,0,0.00037,0.00035,0.045,0.022,0.024,0.0056,0.32,0.32,0.032,3.9e-07,3.4e-07,3.2e-06,0.029,0.029,0.00014,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 +26990000,-0.27,0.016,-0.0069,0.96,0.056,-0.026,0.021,0.018,-0.064,-3.5,-0.0014,-0.0058,-0.00035,0.075,-0.032,-0.13,-0.11,-0.024,0.5,0.083,-0.034,-0.066,0,0,0.00037,0.00035,0.045,0.021,0.023,0.0055,0.31,0.32,0.031,3.8e-07,3.4e-07,3.1e-06,0.029,0.029,0.00014,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 +27090000,-0.27,0.016,-0.0067,0.96,0.058,-0.033,0.025,0.024,-0.067,-3.5,-0.0014,-0.0058,-0.00035,0.075,-0.032,-0.13,-0.11,-0.024,0.5,0.083,-0.034,-0.066,0,0,0.00037,0.00035,0.045,0.022,0.024,0.0055,0.33,0.33,0.031,3.8e-07,3.4e-07,3.1e-06,0.029,0.029,0.00014,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 +27190000,-0.27,0.016,-0.0068,0.96,0.058,-0.036,0.027,0.014,-0.069,-3.5,-0.0014,-0.0058,-0.00034,0.075,-0.032,-0.13,-0.11,-0.024,0.5,0.083,-0.034,-0.066,0,0,0.00037,0.00035,0.045,0.021,0.023,0.0055,0.32,0.33,0.031,3.8e-07,3.3e-07,3.1e-06,0.029,0.029,0.00014,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 +27290000,-0.27,0.017,-0.0069,0.96,0.066,-0.041,0.14,0.02,-0.073,-3.5,-0.0014,-0.0058,-0.00035,0.075,-0.033,-0.13,-0.11,-0.024,0.5,0.083,-0.034,-0.066,0,0,0.00038,0.00035,0.045,0.022,0.024,0.0055,0.33,0.34,0.031,3.8e-07,3.3e-07,3.1e-06,0.029,0.029,0.00014,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 +27390000,-0.27,0.019,-0.0081,0.96,0.07,-0.034,0.46,0.0048,-0.026,-3.5,-0.0014,-0.0058,-0.00032,0.075,-0.033,-0.13,-0.11,-0.024,0.5,0.083,-0.034,-0.066,0,0,0.00039,0.00035,0.045,0.015,0.017,0.0055,0.15,0.15,0.031,3.7e-07,3.3e-07,3.1e-06,0.029,0.029,0.00013,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 +27490000,-0.27,0.021,-0.0093,0.96,0.076,-0.037,0.78,0.012,-0.029,-3.5,-0.0014,-0.0058,-0.00033,0.075,-0.033,-0.13,-0.11,-0.024,0.5,0.083,-0.034,-0.066,0,0,0.0004,0.00036,0.045,0.016,0.017,0.0055,0.15,0.15,0.031,3.7e-07,3.3e-07,3.1e-06,0.029,0.029,0.00013,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 +27590000,-0.27,0.02,-0.0093,0.96,0.067,-0.039,0.87,0.0064,-0.02,-3.4,-0.0014,-0.0058,-0.00032,0.074,-0.034,-0.13,-0.11,-0.024,0.5,0.083,-0.034,-0.066,0,0,0.0004,0.00036,0.045,0.014,0.015,0.0055,0.096,0.097,0.031,3.7e-07,3.2e-07,3e-06,0.029,0.029,0.00013,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 +27690000,-0.27,0.017,-0.0084,0.96,0.061,-0.036,0.78,0.013,-0.024,-3.3,-0.0014,-0.0058,-0.00031,0.075,-0.034,-0.13,-0.11,-0.024,0.5,0.083,-0.034,-0.066,0,0,0.00038,0.00035,0.045,0.014,0.016,0.0055,0.1,0.1,0.031,3.7e-07,3.2e-07,3e-06,0.029,0.029,0.00013,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 +27790000,-0.27,0.016,-0.0072,0.96,0.058,-0.034,0.77,0.01,-0.019,-3.2,-0.0014,-0.0058,-0.0003,0.074,-0.033,-0.13,-0.11,-0.024,0.5,0.083,-0.034,-0.066,0,0,0.00037,0.00035,0.045,0.013,0.015,0.0054,0.073,0.074,0.031,3.6e-07,3.2e-07,3e-06,0.029,0.029,0.00013,0.0011,5.6e-05,0.0012,0.0011,0.00065,0.0012,1,1 +27890000,-0.27,0.016,-0.0068,0.96,0.064,-0.04,0.81,0.016,-0.023,-3.2,-0.0014,-0.0058,-0.0003,0.074,-0.033,-0.13,-0.11,-0.024,0.5,0.083,-0.034,-0.066,0,0,0.00037,0.00035,0.045,0.014,0.016,0.0055,0.076,0.077,0.031,3.6e-07,3.2e-07,3e-06,0.029,0.029,0.00013,0.0011,5.6e-05,0.0012,0.0011,0.00065,0.0012,1,1 +27990000,-0.27,0.016,-0.0072,0.96,0.064,-0.042,0.8,0.019,-0.026,-3.1,-0.0014,-0.0058,-0.00029,0.074,-0.033,-0.13,-0.11,-0.024,0.5,0.083,-0.034,-0.066,0,0,0.00038,0.00035,0.045,0.014,0.016,0.0054,0.079,0.079,0.031,3.6e-07,3.2e-07,3e-06,0.029,0.029,0.00013,0.0011,5.6e-05,0.0012,0.0011,0.00065,0.0012,1,1 +28090000,-0.27,0.016,-0.0075,0.96,0.068,-0.043,0.8,0.026,-0.03,-3,-0.0014,-0.0058,-0.00028,0.074,-0.033,-0.13,-0.11,-0.024,0.5,0.083,-0.034,-0.066,0,0,0.00038,0.00035,0.045,0.015,0.017,0.0054,0.082,0.083,0.031,3.6e-07,3.1e-07,3e-06,0.029,0.029,0.00013,0.0011,5.6e-05,0.0012,0.0011,0.00065,0.0012,1,1 +28190000,-0.27,0.016,-0.0069,0.96,0.065,-0.041,0.81,0.027,-0.032,-2.9,-0.0013,-0.0058,-0.00028,0.074,-0.033,-0.13,-0.11,-0.024,0.5,0.083,-0.034,-0.066,0,0,0.00038,0.00035,0.045,0.015,0.017,0.0054,0.085,0.085,0.031,3.5e-07,3.1e-07,2.9e-06,0.029,0.029,0.00013,0.0011,5.6e-05,0.0012,0.0011,0.00065,0.0012,1,1 +28290000,-0.27,0.017,-0.0064,0.96,0.069,-0.044,0.81,0.033,-0.037,-2.9,-0.0013,-0.0058,-0.00027,0.074,-0.033,-0.13,-0.11,-0.024,0.5,0.083,-0.034,-0.066,0,0,0.00038,0.00035,0.045,0.016,0.018,0.0054,0.089,0.09,0.031,3.5e-07,3.1e-07,2.9e-06,0.029,0.029,0.00013,0.0011,5.6e-05,0.0012,0.0011,0.00065,0.0012,1,1 +28390000,-0.27,0.017,-0.0063,0.96,0.07,-0.046,0.81,0.035,-0.038,-2.8,-0.0013,-0.0058,-0.00025,0.074,-0.033,-0.13,-0.11,-0.024,0.5,0.083,-0.034,-0.066,0,0,0.00039,0.00035,0.045,0.015,0.017,0.0054,0.091,0.092,0.031,3.5e-07,3.1e-07,2.9e-06,0.029,0.029,0.00013,0.0011,5.6e-05,0.0012,0.0011,0.00065,0.0012,1,1 +28490000,-0.27,0.018,-0.0066,0.96,0.073,-0.049,0.81,0.043,-0.042,-2.7,-0.0013,-0.0058,-0.00025,0.074,-0.033,-0.13,-0.11,-0.024,0.5,0.083,-0.034,-0.066,0,0,0.00039,0.00035,0.045,0.016,0.018,0.0054,0.095,0.097,0.031,3.5e-07,3.1e-07,2.9e-06,0.029,0.029,0.00013,0.0011,5.6e-05,0.0012,0.0011,0.00065,0.0012,1,1 +28590000,-0.27,0.018,-0.0066,0.96,0.065,-0.049,0.81,0.043,-0.045,-2.6,-0.0013,-0.0058,-0.00023,0.073,-0.033,-0.13,-0.11,-0.024,0.5,0.083,-0.034,-0.066,0,0,0.00039,0.00035,0.045,0.016,0.018,0.0054,0.098,0.099,0.031,3.4e-07,3e-07,2.9e-06,0.029,0.029,0.00013,0.0011,5.5e-05,0.0012,0.0011,0.00065,0.0012,1,1 +28690000,-0.27,0.017,-0.0064,0.96,0.064,-0.05,0.81,0.05,-0.05,-2.6,-0.0013,-0.0058,-0.00024,0.073,-0.033,-0.12,-0.11,-0.024,0.5,0.083,-0.034,-0.066,0,0,0.00039,0.00035,0.045,0.017,0.019,0.0054,0.1,0.1,0.031,3.4e-07,3e-07,2.9e-06,0.029,0.029,0.00013,0.0011,5.5e-05,0.0012,0.0011,0.00065,0.0012,1,1 +28790000,-0.27,0.017,-0.0058,0.96,0.062,-0.049,0.81,0.051,-0.049,-2.5,-0.0013,-0.0058,-0.00021,0.073,-0.033,-0.12,-0.11,-0.024,0.5,0.083,-0.034,-0.066,0,0,0.00038,0.00034,0.045,0.017,0.019,0.0053,0.11,0.11,0.031,3.4e-07,3e-07,2.9e-06,0.029,0.029,0.00013,0.0011,5.5e-05,0.0012,0.0011,0.00065,0.0012,1,1 +28890000,-0.27,0.016,-0.0056,0.96,0.065,-0.051,0.81,0.057,-0.054,-2.4,-0.0013,-0.0058,-0.00021,0.073,-0.033,-0.12,-0.11,-0.024,0.5,0.083,-0.034,-0.066,0,0,0.00038,0.00034,0.045,0.017,0.02,0.0054,0.11,0.11,0.031,3.4e-07,3e-07,2.8e-06,0.029,0.029,0.00013,0.0011,5.5e-05,0.0012,0.0011,0.00065,0.0012,1,1 +28990000,-0.27,0.016,-0.0054,0.96,0.063,-0.048,0.81,0.058,-0.054,-2.3,-0.0013,-0.0058,-0.00019,0.073,-0.033,-0.12,-0.11,-0.024,0.5,0.082,-0.034,-0.066,0,0,0.00039,0.00034,0.045,0.017,0.019,0.0053,0.11,0.12,0.031,3.4e-07,3e-07,2.8e-06,0.029,0.029,0.00012,0.0011,5.5e-05,0.0012,0.0011,0.00065,0.0012,1,1 +29090000,-0.27,0.017,-0.0052,0.96,0.067,-0.05,0.81,0.066,-0.059,-2.3,-0.0013,-0.0058,-0.00019,0.073,-0.033,-0.12,-0.11,-0.024,0.5,0.082,-0.034,-0.066,0,0,0.00039,0.00034,0.045,0.018,0.02,0.0053,0.12,0.12,0.031,3.4e-07,3e-07,2.8e-06,0.029,0.029,0.00012,0.0011,5.5e-05,0.0012,0.0011,0.00065,0.0012,1,1 +29190000,-0.27,0.017,-0.0051,0.96,0.067,-0.049,0.8,0.067,-0.058,-2.2,-0.0013,-0.0058,-0.00017,0.073,-0.033,-0.12,-0.11,-0.024,0.5,0.082,-0.034,-0.066,0,0,0.00039,0.00034,0.045,0.018,0.02,0.0053,0.12,0.12,0.031,3.3e-07,2.9e-07,2.8e-06,0.029,0.029,0.00012,0.0011,5.5e-05,0.0012,0.0011,0.00065,0.0012,1,1 +29290000,-0.27,0.017,-0.0054,0.96,0.071,-0.054,0.81,0.076,-0.062,-2.1,-0.0013,-0.0058,-0.00017,0.073,-0.033,-0.12,-0.11,-0.024,0.5,0.082,-0.034,-0.066,0,0,0.00039,0.00034,0.045,0.018,0.021,0.0053,0.13,0.13,0.031,3.3e-07,2.9e-07,2.8e-06,0.029,0.029,0.00012,0.0011,5.5e-05,0.0012,0.0011,0.00065,0.0012,1,1 +29390000,-0.27,0.016,-0.0059,0.96,0.067,-0.051,0.81,0.074,-0.059,-2,-0.0013,-0.0058,-0.00014,0.073,-0.033,-0.12,-0.11,-0.024,0.5,0.082,-0.034,-0.066,0,0,0.00038,0.00034,0.045,0.018,0.02,0.0053,0.13,0.13,0.031,3.3e-07,2.9e-07,2.8e-06,0.029,0.029,0.00012,0.0011,5.5e-05,0.0012,0.0011,0.00065,0.0012,1,1 +29490000,-0.27,0.016,-0.0059,0.96,0.07,-0.052,0.81,0.081,-0.065,-2,-0.0013,-0.0058,-0.00013,0.073,-0.033,-0.12,-0.11,-0.024,0.5,0.082,-0.034,-0.066,0,0,0.00038,0.00034,0.045,0.019,0.021,0.0053,0.14,0.14,0.031,3.3e-07,2.9e-07,2.7e-06,0.029,0.029,0.00012,0.0011,5.5e-05,0.0012,0.0011,0.00065,0.0012,1,1 +29590000,-0.27,0.016,-0.0058,0.96,0.068,-0.05,0.81,0.08,-0.063,-1.9,-0.0013,-0.0058,-0.00011,0.072,-0.033,-0.12,-0.11,-0.024,0.5,0.082,-0.034,-0.066,0,0,0.00038,0.00034,0.044,0.018,0.021,0.0053,0.14,0.14,0.031,3.2e-07,2.9e-07,2.7e-06,0.029,0.029,0.00012,0.0011,5.5e-05,0.0012,0.0011,0.00065,0.0012,1,1 +29690000,-0.27,0.016,-0.0058,0.96,0.072,-0.049,0.81,0.087,-0.068,-1.8,-0.0013,-0.0058,-9.8e-05,0.072,-0.033,-0.12,-0.11,-0.024,0.5,0.082,-0.034,-0.066,0,0,0.00038,0.00034,0.044,0.019,0.022,0.0053,0.15,0.15,0.031,3.2e-07,2.9e-07,2.7e-06,0.029,0.029,0.00012,0.0011,5.5e-05,0.0012,0.0011,0.00064,0.0012,1,1 +29790000,-0.27,0.016,-0.0056,0.96,0.069,-0.042,0.8,0.084,-0.064,-1.7,-0.0013,-0.0058,-6.9e-05,0.072,-0.033,-0.12,-0.11,-0.024,0.5,0.082,-0.034,-0.066,0,0,0.00039,0.00034,0.044,0.019,0.021,0.0053,0.15,0.15,0.031,3.2e-07,2.8e-07,2.7e-06,0.029,0.029,0.00012,0.0011,5.5e-05,0.0012,0.0011,0.00064,0.0012,1,1 +29890000,-0.27,0.016,-0.005,0.96,0.07,-0.044,0.8,0.092,-0.069,-1.7,-0.0013,-0.0058,-6.2e-05,0.072,-0.033,-0.12,-0.11,-0.024,0.5,0.082,-0.034,-0.066,0,0,0.00039,0.00034,0.044,0.02,0.022,0.0053,0.15,0.16,0.031,3.2e-07,2.8e-07,2.7e-06,0.029,0.029,0.00012,0.0011,5.5e-05,0.0012,0.0011,0.00064,0.0012,1,1 +29990000,-0.27,0.016,-0.0052,0.96,0.065,-0.041,0.8,0.087,-0.068,-1.6,-0.0013,-0.0058,-4.8e-05,0.072,-0.034,-0.12,-0.11,-0.024,0.5,0.082,-0.034,-0.066,0,0,0.00039,0.00034,0.044,0.019,0.021,0.0052,0.16,0.16,0.03,3.2e-07,2.8e-07,2.6e-06,0.029,0.029,0.00012,0.0011,5.5e-05,0.0012,0.0011,0.00064,0.0012,1,1 +30090000,-0.27,0.017,-0.0053,0.96,0.067,-0.041,0.8,0.094,-0.07,-1.5,-0.0013,-0.0058,-6.2e-05,0.072,-0.034,-0.12,-0.11,-0.024,0.5,0.082,-0.034,-0.066,0,0,0.00039,0.00034,0.044,0.02,0.022,0.0052,0.16,0.17,0.03,3.2e-07,2.8e-07,2.6e-06,0.029,0.029,0.00012,0.0011,5.5e-05,0.0012,0.0011,0.00064,0.0012,1,1 +30190000,-0.27,0.016,-0.0054,0.96,0.062,-0.034,0.8,0.089,-0.061,-1.5,-0.0013,-0.0058,-5e-05,0.072,-0.034,-0.12,-0.11,-0.024,0.5,0.082,-0.034,-0.066,0,0,0.00039,0.00034,0.044,0.019,0.021,0.0052,0.17,0.17,0.031,3.1e-07,2.8e-07,2.6e-06,0.029,0.029,0.00012,0.0011,5.5e-05,0.0012,0.0011,0.00064,0.0012,1,1 +30290000,-0.27,0.016,-0.0054,0.96,0.062,-0.034,0.8,0.096,-0.065,-1.4,-0.0013,-0.0058,-4.9e-05,0.072,-0.034,-0.12,-0.11,-0.024,0.5,0.082,-0.034,-0.066,0,0,0.00039,0.00034,0.044,0.02,0.022,0.0052,0.17,0.18,0.03,3.1e-07,2.8e-07,2.6e-06,0.029,0.029,0.00012,0.0011,5.5e-05,0.0012,0.0011,0.00064,0.0012,1,1 +30390000,-0.27,0.016,-0.0054,0.96,0.059,-0.029,0.8,0.095,-0.059,-1.3,-0.0013,-0.0058,-1.9e-05,0.072,-0.035,-0.12,-0.11,-0.024,0.5,0.082,-0.034,-0.066,0,0,0.00039,0.00034,0.044,0.019,0.022,0.0052,0.18,0.18,0.03,3.1e-07,2.7e-07,2.6e-06,0.029,0.029,0.00012,0.0011,5.5e-05,0.0012,0.0011,0.00064,0.0012,1,1 +30490000,-0.27,0.016,-0.0054,0.96,0.063,-0.028,0.8,0.1,-0.062,-1.2,-0.0013,-0.0058,-1.2e-05,0.072,-0.035,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.066,0,0,0.00039,0.00034,0.044,0.02,0.023,0.0052,0.18,0.19,0.031,3.1e-07,2.7e-07,2.6e-06,0.029,0.029,0.00012,0.0011,5.5e-05,0.0012,0.0011,0.00064,0.0012,1,1 +30590000,-0.27,0.017,-0.0056,0.96,0.061,-0.026,0.8,0.097,-0.058,-1.2,-0.0013,-0.0058,1.2e-05,0.072,-0.035,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.067,0,0,0.00039,0.00034,0.044,0.02,0.022,0.0052,0.18,0.19,0.03,3e-07,2.7e-07,2.5e-06,0.029,0.029,0.00012,0.0011,5.5e-05,0.0012,0.0011,0.00064,0.0012,1,1 +30690000,-0.27,0.017,-0.006,0.96,0.058,-0.026,0.8,0.1,-0.061,-1.1,-0.0013,-0.0058,1.1e-05,0.072,-0.035,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.067,0,0,0.00039,0.00035,0.044,0.02,0.023,0.0052,0.19,0.2,0.03,3e-07,2.7e-07,2.5e-06,0.029,0.029,0.00012,0.0011,5.5e-05,0.0012,0.0011,0.00064,0.0012,1,1 +30790000,-0.27,0.016,-0.0058,0.96,0.052,-0.016,0.8,0.095,-0.049,-1,-0.0012,-0.0058,3.6e-05,0.072,-0.035,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.067,0,0,0.00039,0.00034,0.044,0.02,0.022,0.0052,0.19,0.2,0.03,3e-07,2.7e-07,2.5e-06,0.029,0.029,0.00012,0.0011,5.5e-05,0.0012,0.0011,0.00064,0.0012,1,1 +30890000,-0.27,0.016,-0.0052,0.96,0.051,-0.012,0.79,0.099,-0.05,-0.95,-0.0012,-0.0058,2.6e-05,0.072,-0.036,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.067,0,0,0.00039,0.00034,0.044,0.021,0.023,0.0052,0.2,0.21,0.03,3e-07,2.7e-07,2.5e-06,0.029,0.029,0.00012,0.0011,5.5e-05,0.0012,0.0011,0.00064,0.0012,1,1 +30990000,-0.27,0.016,-0.0054,0.96,0.046,-0.01,0.79,0.095,-0.048,-0.88,-0.0012,-0.0058,3e-05,0.073,-0.036,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.067,0,0,0.00039,0.00034,0.044,0.02,0.022,0.0052,0.2,0.21,0.03,3e-07,2.7e-07,2.5e-06,0.029,0.029,0.00012,0.0011,5.5e-05,0.0012,0.0011,0.00064,0.0012,1,1 +31090000,-0.27,0.016,-0.0055,0.96,0.045,-0.0087,0.79,0.099,-0.049,-0.81,-0.0012,-0.0058,2.2e-05,0.073,-0.036,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.067,0,0,0.00039,0.00034,0.044,0.021,0.023,0.0052,0.21,0.22,0.03,3e-07,2.7e-07,2.5e-06,0.029,0.029,0.00012,0.0011,5.5e-05,0.0012,0.0011,0.00064,0.0012,1,1 +31190000,-0.27,0.017,-0.0057,0.96,0.042,-0.0051,0.8,0.093,-0.044,-0.74,-0.0012,-0.0058,4.6e-05,0.073,-0.036,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.067,0,0,0.00039,0.00034,0.044,0.02,0.022,0.0052,0.21,0.22,0.03,2.9e-07,2.6e-07,2.4e-06,0.029,0.029,0.00012,0.0011,5.5e-05,0.0012,0.0011,0.00064,0.0012,1,1 +31290000,-0.27,0.017,-0.0059,0.96,0.039,-0.0033,0.8,0.096,-0.045,-0.67,-0.0012,-0.0058,5.2e-05,0.073,-0.037,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.067,0,0,0.00039,0.00034,0.044,0.021,0.023,0.0052,0.22,0.23,0.03,2.9e-07,2.6e-07,2.4e-06,0.029,0.029,0.00011,0.0011,5.5e-05,0.0012,0.0011,0.00064,0.0012,1,1 +31390000,-0.27,0.016,-0.0057,0.96,0.035,0.0017,0.8,0.09,-0.042,-0.59,-0.0012,-0.0058,5.1e-05,0.073,-0.037,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.067,0,0,0.00039,0.00034,0.044,0.02,0.022,0.0051,0.22,0.23,0.03,2.9e-07,2.6e-07,2.4e-06,0.029,0.029,0.00011,0.0011,5.5e-05,0.0012,0.0011,0.00064,0.0012,1,1 +31490000,-0.27,0.017,-0.0054,0.96,0.036,0.0051,0.8,0.095,-0.041,-0.52,-0.0012,-0.0058,4.8e-05,0.073,-0.037,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.067,0,0,0.00039,0.00034,0.044,0.021,0.023,0.0052,0.23,0.24,0.03,2.9e-07,2.6e-07,2.4e-06,0.029,0.029,0.00011,0.0011,5.5e-05,0.0012,0.0011,0.00063,0.0012,1,1 +31590000,-0.27,0.017,-0.0052,0.96,0.036,0.007,0.8,0.092,-0.037,-0.45,-0.0012,-0.0058,6.1e-05,0.073,-0.037,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.067,0,0,0.00039,0.00034,0.044,0.02,0.022,0.0051,0.23,0.24,0.03,2.9e-07,2.6e-07,2.4e-06,0.029,0.029,0.00011,0.0011,5.5e-05,0.0012,0.0011,0.00063,0.0012,1,1 +31690000,-0.27,0.017,-0.0052,0.96,0.04,0.0082,0.8,0.097,-0.037,-0.38,-0.0012,-0.0058,7.1e-05,0.073,-0.037,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.067,0,0,0.0004,0.00034,0.043,0.021,0.023,0.0051,0.24,0.25,0.03,2.9e-07,2.6e-07,2.4e-06,0.029,0.029,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 +31790000,-0.27,0.018,-0.0054,0.96,0.034,0.014,0.8,0.093,-0.027,-0.3,-0.0012,-0.0058,9e-05,0.073,-0.038,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.067,0,0,0.0004,0.00034,0.043,0.02,0.022,0.0051,0.24,0.25,0.03,2.9e-07,2.6e-07,2.3e-06,0.029,0.029,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 +31890000,-0.27,0.018,-0.0052,0.96,0.032,0.016,0.8,0.097,-0.025,-0.23,-0.0012,-0.0058,9.4e-05,0.073,-0.038,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.067,0,0,0.0004,0.00034,0.043,0.021,0.023,0.0051,0.25,0.26,0.03,2.9e-07,2.6e-07,2.3e-06,0.029,0.029,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 +31990000,-0.27,0.017,-0.0055,0.96,0.029,0.017,0.79,0.094,-0.02,-0.17,-0.0012,-0.0058,9.2e-05,0.073,-0.038,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.067,0,0,0.0004,0.00034,0.043,0.02,0.022,0.0051,0.25,0.26,0.03,2.8e-07,2.5e-07,2.3e-06,0.029,0.029,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 +32090000,-0.27,0.017,-0.0059,0.96,0.03,0.021,0.8,0.098,-0.018,-0.096,-0.0012,-0.0058,9.2e-05,0.073,-0.038,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.067,0,0,0.00039,0.00034,0.043,0.021,0.023,0.0051,0.26,0.27,0.03,2.8e-07,2.5e-07,2.3e-06,0.029,0.029,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 +32190000,-0.27,0.017,-0.0061,0.96,0.027,0.029,0.8,0.093,-0.0092,-0.027,-0.0012,-0.0058,9.7e-05,0.074,-0.039,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.067,0,0,0.00039,0.00034,0.043,0.02,0.022,0.0051,0.26,0.27,0.03,2.8e-07,2.5e-07,2.3e-06,0.029,0.029,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 +32290000,-0.27,0.017,-0.006,0.96,0.027,0.031,0.8,0.096,-0.0063,0.042,-0.0012,-0.0058,0.0001,0.074,-0.039,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.067,0,0,0.0004,0.00034,0.043,0.021,0.023,0.0051,0.27,0.28,0.03,2.8e-07,2.5e-07,2.3e-06,0.029,0.029,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 +32390000,-0.27,0.017,-0.0062,0.96,0.023,0.033,0.79,0.092,-0.0029,0.12,-0.0012,-0.0058,0.0001,0.074,-0.039,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.067,0,0,0.0004,0.00034,0.043,0.02,0.022,0.0051,0.27,0.28,0.03,2.8e-07,2.5e-07,2.2e-06,0.029,0.029,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 +32490000,-0.27,0.016,-0.0092,0.96,-0.017,0.092,-0.077,0.091,0.0054,0.12,-0.0012,-0.0058,9.6e-05,0.074,-0.039,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.067,0,0,0.00038,0.00035,0.043,0.022,0.025,0.0051,0.28,0.29,0.03,2.8e-07,2.5e-07,2.2e-06,0.029,0.029,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 +32590000,-0.27,0.016,-0.0092,0.96,-0.014,0.09,-0.08,0.091,-0.0025,0.1,-0.0012,-0.0058,8.5e-05,0.074,-0.039,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.067,0,0,0.00038,0.00035,0.043,0.021,0.023,0.0051,0.28,0.29,0.03,2.8e-07,2.5e-07,2.2e-06,0.029,0.029,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 +32690000,-0.27,0.016,-0.0092,0.96,-0.01,0.097,-0.081,0.09,0.0069,0.088,-0.0012,-0.0058,8.5e-05,0.074,-0.039,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.067,0,0,0.00038,0.00035,0.043,0.022,0.024,0.0051,0.29,0.3,0.03,2.8e-07,2.5e-07,2.2e-06,0.029,0.029,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 +32790000,-0.27,0.016,-0.009,0.96,-0.0062,0.095,-0.082,0.092,-0.0016,0.073,-0.0012,-0.0058,7.5e-05,0.074,-0.039,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.067,0,0,0.00037,0.00035,0.043,0.021,0.023,0.0051,0.29,0.3,0.03,2.7e-07,2.5e-07,2.2e-06,0.029,0.029,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 +32890000,-0.27,0.016,-0.009,0.96,-0.0066,0.1,-0.084,0.09,0.0078,0.058,-0.0012,-0.0058,8.1e-05,0.074,-0.039,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.067,0,0,0.00038,0.00035,0.043,0.022,0.024,0.0051,0.3,0.31,0.03,2.7e-07,2.5e-07,2.2e-06,0.029,0.029,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 +32990000,-0.27,0.016,-0.0088,0.96,-0.0026,0.095,-0.083,0.091,-0.006,0.044,-0.0012,-0.0058,6.8e-05,0.074,-0.039,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.067,0,0,0.00037,0.00035,0.043,0.021,0.023,0.0051,0.3,0.31,0.03,2.7e-07,2.5e-07,2.1e-06,0.029,0.029,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 +33090000,-0.27,0.016,-0.0088,0.96,0.0013,0.1,-0.08,0.091,0.0037,0.037,-0.0012,-0.0058,6.8e-05,0.074,-0.039,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.067,0,0,0.00037,0.00035,0.043,0.021,0.024,0.0051,0.31,0.32,0.03,2.7e-07,2.5e-07,2.1e-06,0.029,0.029,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 +33190000,-0.27,0.016,-0.0086,0.96,0.0057,0.096,-0.079,0.092,-0.012,0.029,-0.0012,-0.0058,4e-05,0.074,-0.038,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.067,0,0,0.00037,0.00035,0.043,0.021,0.023,0.0051,0.31,0.32,0.03,2.7e-07,2.4e-07,2.1e-06,0.029,0.029,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 +33290000,-0.27,0.016,-0.0087,0.96,0.0098,0.099,-0.079,0.093,-0.0027,0.021,-0.0013,-0.0058,5.7e-05,0.074,-0.038,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.066,0,0,0.00037,0.00035,0.043,0.021,0.024,0.0051,0.32,0.33,0.03,2.7e-07,2.4e-07,2.1e-06,0.029,0.029,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 +33390000,-0.27,0.016,-0.0086,0.96,0.014,0.095,-0.077,0.092,-0.012,0.012,-0.0013,-0.0058,4.6e-05,0.075,-0.037,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.066,0,0,0.00037,0.00034,0.043,0.021,0.023,0.0051,0.32,0.33,0.03,2.7e-07,2.4e-07,2.1e-06,0.029,0.028,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 +33490000,-0.27,0.016,-0.0086,0.96,0.02,0.099,-0.076,0.095,-0.0019,0.0028,-0.0013,-0.0058,5.4e-05,0.075,-0.037,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.066,0,0,0.00037,0.00034,0.043,0.021,0.024,0.0051,0.33,0.34,0.03,2.7e-07,2.4e-07,2.1e-06,0.029,0.028,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 +33590000,-0.27,0.016,-0.0084,0.96,0.023,0.096,-0.073,0.094,-0.015,-0.0051,-0.0013,-0.0058,4.8e-05,0.075,-0.036,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.067,0,0,0.00037,0.00034,0.043,0.02,0.023,0.005,0.33,0.34,0.03,2.6e-07,2.4e-07,2e-06,0.029,0.028,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 +33690000,-0.27,0.016,-0.0084,0.96,0.026,0.099,-0.074,0.095,-0.0061,-0.013,-0.0013,-0.0058,5.3e-05,0.075,-0.036,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.066,0,0,0.00037,0.00034,0.043,0.021,0.024,0.0051,0.34,0.35,0.03,2.7e-07,2.4e-07,2e-06,0.029,0.028,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 +33790000,-0.27,0.016,-0.0083,0.96,0.029,0.096,-0.068,0.092,-0.02,-0.02,-0.0013,-0.0058,3.3e-05,0.076,-0.036,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.066,0,0,0.00037,0.00034,0.043,0.02,0.023,0.005,0.34,0.35,0.03,2.6e-07,2.4e-07,2e-06,0.029,0.028,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 +33890000,-0.27,0.016,-0.0083,0.96,0.033,0.097,-0.068,0.095,-0.01,-0.027,-0.0013,-0.0058,4.8e-05,0.076,-0.036,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.066,0,0,0.00037,0.00034,0.043,0.021,0.024,0.0051,0.35,0.36,0.03,2.6e-07,2.4e-07,2e-06,0.029,0.028,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 +33990000,-0.27,0.016,-0.0082,0.96,0.036,0.095,-0.064,0.094,-0.019,-0.03,-0.0013,-0.0057,3.3e-05,0.076,-0.035,-0.12,-0.11,-0.024,0.5,0.081,-0.035,-0.066,0,0,0.00037,0.00034,0.043,0.02,0.023,0.005,0.35,0.36,0.03,2.6e-07,2.4e-07,2e-06,0.029,0.028,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 +34090000,-0.27,0.016,-0.0081,0.96,0.039,0.1,-0.063,0.097,-0.0094,-0.035,-0.0013,-0.0057,3.6e-05,0.076,-0.035,-0.12,-0.11,-0.024,0.5,0.081,-0.035,-0.066,0,0,0.00037,0.00034,0.043,0.021,0.024,0.0051,0.36,0.37,0.03,2.6e-07,2.4e-07,2e-06,0.029,0.028,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 +34190000,-0.27,0.016,-0.0081,0.96,0.04,0.096,-0.06,0.092,-0.021,-0.038,-0.0013,-0.0057,3e-05,0.077,-0.035,-0.12,-0.11,-0.024,0.5,0.081,-0.035,-0.067,0,0,0.00037,0.00034,0.043,0.02,0.022,0.005,0.36,0.36,0.03,2.6e-07,2.4e-07,2e-06,0.028,0.028,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 +34290000,-0.26,0.016,-0.0079,0.96,0.041,0.1,-0.059,0.096,-0.012,-0.044,-0.0013,-0.0057,4e-05,0.077,-0.035,-0.12,-0.11,-0.024,0.5,0.08,-0.035,-0.066,0,0,0.00037,0.00034,0.043,0.021,0.023,0.005,0.37,0.38,0.03,2.6e-07,2.4e-07,2e-06,0.028,0.028,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 +34390000,-0.26,0.016,-0.0078,0.96,0.043,0.095,-0.054,0.091,-0.023,-0.048,-0.0013,-0.0057,3e-05,0.077,-0.034,-0.12,-0.11,-0.024,0.5,0.08,-0.035,-0.067,0,0,0.00037,0.00034,0.042,0.02,0.022,0.005,0.37,0.37,0.03,2.6e-07,2.4e-07,1.9e-06,0.028,0.028,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00062,0.0012,1,1 +34490000,-0.26,0.016,-0.0079,0.96,0.046,0.099,-0.052,0.095,-0.014,-0.051,-0.0013,-0.0057,4.2e-05,0.077,-0.034,-0.12,-0.11,-0.024,0.5,0.08,-0.035,-0.067,0,0,0.00037,0.00034,0.042,0.021,0.023,0.005,0.38,0.39,0.03,2.6e-07,2.4e-07,1.9e-06,0.028,0.028,0.00011,0.0011,5.3e-05,0.0012,0.0011,0.00062,0.0012,1,1 +34590000,-0.26,0.016,-0.0078,0.96,0.049,0.092,0.74,0.09,-0.028,-0.022,-0.0013,-0.0057,3e-05,0.078,-0.034,-0.12,-0.11,-0.024,0.5,0.08,-0.035,-0.067,0,0,0.00036,0.00034,0.042,0.02,0.022,0.005,0.38,0.38,0.03,2.6e-07,2.4e-07,1.9e-06,0.028,0.028,0.00011,0.0011,5.3e-05,0.0012,0.0011,0.00062,0.0012,1,1 +34690000,-0.26,0.015,-0.0078,0.96,0.058,0.092,1.7,0.095,-0.019,0.097,-0.0013,-0.0057,3.5e-05,0.078,-0.034,-0.12,-0.11,-0.024,0.5,0.08,-0.035,-0.067,0,0,0.00036,0.00034,0.042,0.02,0.022,0.0051,0.39,0.4,0.03,2.6e-07,2.4e-07,1.9e-06,0.028,0.028,0.00011,0.0011,5.3e-05,0.0012,0.0011,0.00062,0.0012,1,1 +34790000,-0.26,0.015,-0.0077,0.96,0.063,0.086,2.7,0.088,-0.032,0.28,-0.0013,-0.0057,2.6e-05,0.078,-0.034,-0.12,-0.11,-0.024,0.5,0.08,-0.035,-0.067,0,0,0.00036,0.00034,0.042,0.02,0.021,0.005,0.39,0.39,0.03,2.6e-07,2.4e-07,1.9e-06,0.028,0.028,0.00011,0.0011,5.3e-05,0.0012,0.0011,0.00062,0.0012,1,1 +34890000,-0.26,0.015,-0.0077,0.96,0.071,0.086,3.7,0.094,-0.023,0.57,-0.0013,-0.0057,3.2e-05,0.079,-0.034,-0.12,-0.11,-0.024,0.5,0.08,-0.035,-0.067,0,0,0.00036,0.00034,0.042,0.021,0.023,0.005,0.4,0.41,0.03,2.6e-07,2.4e-07,1.9e-06,0.028,0.028,0.00011,0.0011,5.3e-05,0.0012,0.0011,0.00062,0.0012,1,1 diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp index 482f7671eaed..fe2c3a88ff7c 100644 --- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp +++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp @@ -100,6 +100,11 @@ bool EkfWrapper::isIntendingBetaFusion() const return _ekf->control_status_flags().fuse_beta; } +bool EkfWrapper::isIntendingAirspeedFusion() const +{ + return _ekf->control_status_flags().fuse_aspd; +} + void EkfWrapper::enableGpsFusion() { _ekf_params->gnss_ctrl |= static_cast(GnssCtrl::HPOS) | static_cast(GnssCtrl::VEL); @@ -309,3 +314,13 @@ float EkfWrapper::getMagHeadingNoise() const { return _ekf_params->mag_heading_noise; } + +void EkfWrapper::enableGyroBiasEstimation() +{ + _ekf_params->imu_ctrl |= static_cast(ImuCtrl::GyroBias); +} + +void EkfWrapper::disableGyroBiasEstimation() +{ + _ekf_params->imu_ctrl &= ~static_cast(ImuCtrl::GyroBias); +} diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h index beb43e64e3be..e1d35f093408 100644 --- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h +++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h @@ -73,6 +73,8 @@ class EkfWrapper void disableBetaFusion(); bool isIntendingBetaFusion() const; + bool isIntendingAirspeedFusion() const; + void enableGpsFusion(); void disableGpsFusion(); bool isIntendingGpsFusion() const; @@ -126,6 +128,9 @@ class EkfWrapper float getMagHeadingNoise() const; + void enableGyroBiasEstimation(); + void disableGyroBiasEstimation(); + private: std::shared_ptr _ekf; diff --git a/src/modules/ekf2/test/sensor_simulator/sensor_simulator.cpp b/src/modules/ekf2/test/sensor_simulator/sensor_simulator.cpp index ea485b725873..3d1649317ed7 100644 --- a/src/modules/ekf2/test/sensor_simulator/sensor_simulator.cpp +++ b/src/modules/ekf2/test/sensor_simulator/sensor_simulator.cpp @@ -132,7 +132,7 @@ void SensorSimulator::setSensorDataToDefault() _flow.setData(_flow.dataAtRest()); _gps.setData(_gps.getDefaultGpsData()); _imu.setData(Vector3f{0.0f, 0.0f, -CONSTANTS_ONE_G}, Vector3f{0.0f, 0.0f, 0.0f}); - _mag.setData(Vector3f{0.2f, 0.0f, 0.4f}); + _mag.setData(Vector3f{0.218f, 0.f, 0.43f}); _rng.setData(0.2f, 100); _vio.setData(_vio.dataAtRest()); } @@ -358,7 +358,7 @@ void SensorSimulator::setSensorDataFromTrajectory() // Magnetometer if (_mag.isRunning()) { - const Vector3f world_mag_field = Vector3f{0.2f, 0.0f, 0.4f}; + const Vector3f world_mag_field = Vector3f{0.218f, 0.f, 0.43f}; const Vector3f mag_field_body = R_world_to_body * world_mag_field; _mag.setData(mag_field_body); } @@ -419,7 +419,10 @@ void SensorSimulator::simulateOrientation(Quatf orientation) _R_body_to_world = Dcmf(orientation); const Vector3f world_sensed_gravity = {0.0f, 0.0f, -CONSTANTS_ONE_G}; - const Vector3f world_mag_field = Vector3f{0.2f, 0.0f, 0.4f}; + + // The world mag field Y component is 0 as most unit tests assume no magnetic dectination + const Vector3f world_mag_field = Vector3f{0.218f, 0.f, 0.43f}; + const Vector3f sensed_gravity_body = _R_body_to_world.transpose() * world_sensed_gravity; const Vector3f body_mag_field = _R_body_to_world.transpose() * world_mag_field; diff --git a/src/modules/ekf2/test/test_EKF_accelerometer.cpp b/src/modules/ekf2/test/test_EKF_accelerometer.cpp index 5d18429bc176..255f6597b3f5 100644 --- a/src/modules/ekf2/test/test_EKF_accelerometer.cpp +++ b/src/modules/ekf2/test/test_EKF_accelerometer.cpp @@ -95,7 +95,7 @@ TEST_F(EkfAccelerometerTest, biasEstimatePositive) const float biases[4] = {0.1f, 0.2f, 0.3f, 0.38f}; for (int i = 0; i < 4; i ++) { - testBias(biases[i], 10, 0.03f); + testBias(biases[i], 60, 0.03f); } } @@ -104,7 +104,7 @@ TEST_F(EkfAccelerometerTest, biasEstimateNegative) const float biases[4] = {-0.12f, -0.22f, -0.31, -0.4f}; for (int i = 0; i < 4; i ++) { - testBias(biases[i], 10, 0.03f); + testBias(biases[i], 60, 0.03f); } } diff --git a/src/modules/ekf2/test/test_EKF_airspeed.cpp b/src/modules/ekf2/test/test_EKF_airspeed.cpp index 28964035c96c..d284b959e3be 100644 --- a/src/modules/ekf2/test/test_EKF_airspeed.cpp +++ b/src/modules/ekf2/test/test_EKF_airspeed.cpp @@ -166,3 +166,39 @@ TEST_F(EkfAirspeedTest, testResetWindUsingAirspeed) EXPECT_NEAR(vel_wind_earth(0), vel_wind_expected(0), 1.f); EXPECT_NEAR(vel_wind_earth(1), vel_wind_expected(1), 1.f); } + +TEST_F(EkfAirspeedTest, testAirspeedDeadReckoning) +{ + const Vector3f simulated_velocity_earth(-3.6f, 8.f, 0.0f); + const Vector2f airspeed_body(15.f, 0.0f); + _sensor_simulator.runSeconds(10); + + _ekf->set_in_air_status(true); + _ekf->set_vehicle_at_rest(false); + _ekf->set_is_fixed_wing(true); + + const double latitude_new = -15.0000005; + const double longitude_new = -115.0000005; + const float altitude_new = 1500.0; + + _ekf->setEkfGlobalOrigin(latitude_new, longitude_new, altitude_new); + _ekf->resetGlobalPosToExternalObservation(latitude_new, longitude_new, 50.f, 0); + + // Simulate the fact that the sideslip can start immediately, without + // waiting for a measurement sample. + _ekf_wrapper.enableBetaFusion(); + _sensor_simulator.runSeconds(1.f); + EXPECT_TRUE(_ekf_wrapper.isIntendingBetaFusion()); + + _sensor_simulator.startAirspeedSensor(); + _sensor_simulator._airspeed.setData(airspeed_body(0), airspeed_body(0)); + _sensor_simulator.runSeconds(1.f); + EXPECT_TRUE(_ekf_wrapper.isIntendingAirspeedFusion()); + + EXPECT_TRUE(_ekf_wrapper.isWindVelocityEstimated()); + const Vector3f vel = _ekf->getVelocity(); + EXPECT_NEAR(vel.norm(), airspeed_body.norm(), 1e-3f); + const Vector2f vel_wind_earth = _ekf->getWindVelocity(); + EXPECT_NEAR(vel_wind_earth(0), 0.f, .1f); + EXPECT_NEAR(vel_wind_earth(1), 0.f, .1f); +} diff --git a/src/modules/ekf2/test/test_EKF_basics.cpp b/src/modules/ekf2/test/test_EKF_basics.cpp index ba1c842ebee5..c7a8ffa83643 100644 --- a/src/modules/ekf2/test/test_EKF_basics.cpp +++ b/src/modules/ekf2/test/test_EKF_basics.cpp @@ -107,7 +107,8 @@ TEST_F(EkfBasicsTest, initialControlMode) EXPECT_EQ(0, (int) _ekf->control_status_flags().opt_flow); EXPECT_EQ(1, (int) _ekf->control_status_flags().mag_hdg); EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_3D); - EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_dec); + EXPECT_EQ(1, (int) _ekf->control_status_flags().mag); + EXPECT_EQ(1, (int) _ekf->control_status_flags().mag_dec); EXPECT_EQ(0, (int) _ekf->control_status_flags().in_air); EXPECT_EQ(0, (int) _ekf->control_status_flags().wind); EXPECT_EQ(1, (int) _ekf->control_status_flags().baro_hgt); @@ -161,7 +162,8 @@ TEST_F(EkfBasicsTest, gpsFusion) EXPECT_EQ(0, (int) _ekf->control_status_flags().opt_flow); EXPECT_EQ(1, (int) _ekf->control_status_flags().mag_hdg); EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_3D); - EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_dec); + EXPECT_EQ(1, (int) _ekf->control_status_flags().mag); + EXPECT_EQ(1, (int) _ekf->control_status_flags().mag_dec); EXPECT_EQ(0, (int) _ekf->control_status_flags().in_air); EXPECT_EQ(0, (int) _ekf->control_status_flags().wind); EXPECT_EQ(1, (int) _ekf->control_status_flags().baro_hgt); @@ -191,7 +193,7 @@ TEST_F(EkfBasicsTest, accelBiasEstimation) _sensor_simulator.startGps(); _sensor_simulator.setImuBias(accel_bias_sim, Vector3f(0.0f, 0.0f, 0.0f)); _ekf->set_min_required_gps_health_time(1e6); - _sensor_simulator.runSeconds(30); + _sensor_simulator.runSeconds(60); const Vector3f pos = _ekf->getPosition(); const Vector3f vel = _ekf->getVelocity(); diff --git a/src/modules/ekf2/test/test_EKF_drag_fusion.cpp b/src/modules/ekf2/test/test_EKF_drag_fusion.cpp index ca03a0f5e78f..edd81e640c44 100644 --- a/src/modules/ekf2/test/test_EKF_drag_fusion.cpp +++ b/src/modules/ekf2/test/test_EKF_drag_fusion.cpp @@ -144,8 +144,10 @@ TEST_F(EkfDragFusionTest, testLateralMomentumDrag) predicted_accel(0) = CONSTANTS_ONE_G * sinf(pitch); predicted_accel(1) = - CONSTANTS_ONE_G * sinf(roll); Vector2f wind_speed = predicted_accel / mcoef; - EXPECT_NEAR(vel_wind_earth(0), wind_speed(0), fmaxf(1.0f, 0.1f * fabsf(wind_speed(0)))); - EXPECT_NEAR(vel_wind_earth(1), wind_speed(1), fmaxf(1.0f, 0.1f * fabsf(wind_speed(1)))); + // Note that the wind direction is stightly incorrect heading estimate due to a mismatch between + // the simulated mag field and assumed dectination from the WMM + EXPECT_NEAR(vel_wind_earth(0), wind_speed(0), fmaxf(1.0f, 0.15f * fabsf(wind_speed.norm()))); + EXPECT_NEAR(vel_wind_earth(1), wind_speed(1), fmaxf(1.0f, 0.15f * fabsf(wind_speed.norm()))); }; TEST_F(EkfDragFusionTest, testForwardBluffBodyDrag) diff --git a/src/modules/ekf2/test/test_EKF_externalVision.cpp b/src/modules/ekf2/test/test_EKF_externalVision.cpp index aa8376dc6c95..734d485fbcd3 100644 --- a/src/modules/ekf2/test/test_EKF_externalVision.cpp +++ b/src/modules/ekf2/test/test_EKF_externalVision.cpp @@ -285,7 +285,7 @@ TEST_F(EkfExternalVisionTest, velocityFrameBody) // THEN: As the drone is turned 90 degrees, velocity variance // along local y axis is expected to be bigger const Vector3f velVar_new = _ekf->getVelocityVariance(); - EXPECT_NEAR(velVar_new(1) / velVar_new(0), 70.f, 15.f); + EXPECT_NEAR(velVar_new(1) / velVar_new(0), 30.f, 15.f); const Vector3f vel_earth_est = _ekf->getVelocity(); EXPECT_NEAR(vel_earth_est(0), 0.0f, 0.1f); @@ -319,7 +319,7 @@ TEST_F(EkfExternalVisionTest, velocityFrameLocal) // THEN: Independently on drones heading, velocity variance // along local x axis is expected to be bigger const Vector3f velVar_new = _ekf->getVelocityVariance(); - EXPECT_NEAR(velVar_new(0) / velVar_new(1), 70.f, 15.f); + EXPECT_NEAR(velVar_new(0) / velVar_new(1), 30.f, 15.f); const Vector3f vel_earth_est = _ekf->getVelocity(); EXPECT_NEAR(vel_earth_est(0), 1.0f, 0.1f); diff --git a/src/modules/ekf2/test/test_EKF_flow.cpp b/src/modules/ekf2/test/test_EKF_flow.cpp index 0c541b0df407..9d3df6cbd8d3 100644 --- a/src/modules/ekf2/test/test_EKF_flow.cpp +++ b/src/modules/ekf2/test/test_EKF_flow.cpp @@ -241,7 +241,7 @@ TEST_F(EkfFlowTest, yawMotionCorrectionWithAutopilotGyroData) _sensor_simulator.runSeconds(5.f); // AND WHEN: there is a pure yaw rotation - const Vector3f body_rate(0.f, 0.f, 3.14159f); + const Vector3f body_rate(0.f, 0.f, 2.9f); const Vector3f flow_offset(0.15, -0.05f, 0.2f); _ekf_wrapper.setFlowOffset(flow_offset); @@ -277,6 +277,47 @@ TEST_F(EkfFlowTest, yawMotionCorrectionWithFlowGyroData) _sensor_simulator.runSeconds(5.f); + // AND WHEN: there is a pure yaw rotation + const Vector3f body_rate(0.f, 0.f, 2.9f); + const Vector3f flow_offset(-0.15, 0.05f, 0.2f); + _ekf_wrapper.setFlowOffset(flow_offset); + + const Vector2f simulated_horz_velocity(body_rate % flow_offset); + flowSample flow_sample = _sensor_simulator._flow.dataAtRest(); + setFlowFromHorizontalVelocityAndDistance(flow_sample, simulated_horz_velocity, simulated_distance_to_ground); + + // use flow sensor gyro data + // for clarification of the sign, see definition of flowSample + flow_sample.gyro_rate = -body_rate; + + _sensor_simulator._flow.setData(flow_sample); + _sensor_simulator._imu.setGyroData(body_rate); + _sensor_simulator.runSeconds(10.f); + + // THEN: the flow due to the yaw rotation and the offsets is canceled + // and the velocity estimate stays 0 + const Vector2f estimated_horz_velocity = Vector2f(_ekf->getVelocity()); + EXPECT_NEAR(estimated_horz_velocity(0), 0.f, 0.01f) + << "estimated vel = " << estimated_horz_velocity(0); + EXPECT_NEAR(estimated_horz_velocity(1), 0.f, 0.01f) + << "estimated vel = " << estimated_horz_velocity(1); + _ekf->state().vector().print(); + _ekf->covariances().print(); +} + +TEST_F(EkfFlowTest, yawMotionNoMagFusion) +{ + // WHEN: fusing range finder and optical flow data in air + const float simulated_distance_to_ground = 5.f; + startRangeFinderFusion(simulated_distance_to_ground); + startZeroFlowFusion(); + _ekf_wrapper.setMagFuseTypeNone(); + + _ekf->set_in_air_status(true); + _ekf->set_vehicle_at_rest(false); + + _sensor_simulator.runSeconds(5.f); + // AND WHEN: there is a pure yaw rotation const Vector3f body_rate(0.f, 0.f, 3.14159f); const Vector3f flow_offset(-0.15, 0.05f, 0.2f); @@ -301,4 +342,6 @@ TEST_F(EkfFlowTest, yawMotionCorrectionWithFlowGyroData) << "estimated vel = " << estimated_horz_velocity(0); EXPECT_NEAR(estimated_horz_velocity(1), 0.f, 0.01f) << "estimated vel = " << estimated_horz_velocity(1); + _ekf->state().vector().print(); + _ekf->covariances().print(); } diff --git a/src/modules/ekf2/test/test_EKF_gps_yaw.cpp b/src/modules/ekf2/test/test_EKF_gps_yaw.cpp index 42737306eae3..2c3afc655ac4 100644 --- a/src/modules/ekf2/test/test_EKF_gps_yaw.cpp +++ b/src/modules/ekf2/test/test_EKF_gps_yaw.cpp @@ -81,9 +81,10 @@ class EkfGpsHeadingTest : public ::testing::Test void EkfGpsHeadingTest::runConvergenceScenario(float yaw_offset_rad, float antenna_offset_rad) { // GIVEN: an initial GPS yaw, not aligned with the current one - float gps_heading = matrix::wrap_pi(_ekf_wrapper.getYawAngle() + yaw_offset_rad); + // The yaw antenna offset has already been corrected in the driver + float gps_heading = matrix::wrap_pi(_ekf_wrapper.getYawAngle()); - _sensor_simulator._gps.setYaw(gps_heading); + _sensor_simulator._gps.setYaw(gps_heading); // used to remove the correction to fuse the real measurement _sensor_simulator._gps.setYawOffset(antenna_offset_rad); // WHEN: the GPS yaw fusion is activated @@ -91,7 +92,7 @@ void EkfGpsHeadingTest::runConvergenceScenario(float yaw_offset_rad, float anten _sensor_simulator.runSeconds(5); // THEN: the estimate is reset and stays close to the measurement - checkConvergence(gps_heading, 0.05f); + checkConvergence(gps_heading, 0.01f); } void EkfGpsHeadingTest::checkConvergence(float truth, float tolerance_deg) diff --git a/src/modules/ekf2/test/test_EKF_height_fusion.cpp b/src/modules/ekf2/test/test_EKF_height_fusion.cpp index f83ad67cd4ab..329015dc4a40 100644 --- a/src/modules/ekf2/test/test_EKF_height_fusion.cpp +++ b/src/modules/ekf2/test/test_EKF_height_fusion.cpp @@ -182,7 +182,7 @@ TEST_F(EkfHeightFusionTest, gpsRef) EXPECT_NEAR(baro_status.bias, baro_initial + baro_increment, 1.3f); const BiasEstimator::status &rng_status = _ekf->getRngHgtBiasEstimatorStatus(); - EXPECT_NEAR(rng_status.bias, 0.f, 1.f); + EXPECT_NEAR(rng_status.bias, 0.f, 1.1f); // TODO: why? // BUT WHEN: the GPS jumps by a lot const float gps_step = 100.f; diff --git a/src/modules/ekf2/test/test_EKF_initialization.cpp b/src/modules/ekf2/test/test_EKF_initialization.cpp index d7959489295e..142a4ea3ef73 100644 --- a/src/modules/ekf2/test/test_EKF_initialization.cpp +++ b/src/modules/ekf2/test/test_EKF_initialization.cpp @@ -175,7 +175,33 @@ TEST_F(EkfInitializationTest, initializeWithZeroTiltNotAtRest) _ekf->set_vehicle_at_rest(false); _sensor_simulator.simulateOrientation(quat_sim); //_sensor_simulator.runSeconds(_init_tilt_period); - _sensor_simulator.runSeconds(7); + _sensor_simulator.runSeconds(10); + + EXPECT_TRUE(_ekf->control_status_flags().tilt_align); + + initializedOrienationIsMatchingGroundTruth(quat_sim); + quaternionVarianceBigEnoughAfterOrientationInitialization(0.00001f); + + velocityAndPositionCloseToZero(); + + positionVarianceBigEnoughAfterOrientationInitialization(0.00001f); // Fake position fusion obs var when at rest sq(0.5f) + velocityVarianceBigEnoughAfterOrientationInitialization(0.0001f); + + _sensor_simulator.runSeconds(1.f); + learningCorrectAccelBias(); +} + +TEST_F(EkfInitializationTest, initializeWithTiltNoGyroBiasEstimate) +{ + const float pitch = math::radians(30.0f); + const float roll = math::radians(-20.0f); + const Eulerf euler_angles_sim(roll, pitch, 0.0f); + const Quatf quat_sim(euler_angles_sim); + + _ekf_wrapper.disableGyroBiasEstimation(); + _sensor_simulator.simulateOrientation(quat_sim); + + _sensor_simulator.runSeconds(_init_tilt_period); EXPECT_TRUE(_ekf->control_status_flags().tilt_align); @@ -320,7 +346,7 @@ TEST_F(EkfInitializationTest, initializeWithTiltNotAtRest) _ekf->set_vehicle_at_rest(false); _sensor_simulator.simulateOrientation(quat_sim); //_sensor_simulator.runSeconds(_init_tilt_period); - _sensor_simulator.runSeconds(7); + _sensor_simulator.runSeconds(10); EXPECT_TRUE(_ekf->control_status_flags().tilt_align); diff --git a/src/modules/ekf2/test/test_EKF_mag.cpp b/src/modules/ekf2/test/test_EKF_mag.cpp index 32903e6e6cda..6641b4fe6dc1 100644 --- a/src/modules/ekf2/test/test_EKF_mag.cpp +++ b/src/modules/ekf2/test/test_EKF_mag.cpp @@ -70,7 +70,9 @@ TEST_F(EkfMagTest, fusionStartWithReset) { // GIVEN: some meaningful mag data const float mag_heading = M_PI_F / 3.f; - const Vector3f mag_data(0.2f * cosf(mag_heading), -0.2f * sinf(mag_heading), 0.4f); + const float incl = 63.1f; + const Vector3f mag_data(0.2f * cosf(mag_heading), -0.2f * sinf(mag_heading), + 0.4f * sinf(incl) * sqrtf(0.2f * 0.2f + 0.4f * 0.4f)); _sensor_simulator._mag.setData(mag_data); const int initial_quat_reset_counter = _ekf_wrapper.getQuaternionResetCounter(); @@ -95,7 +97,7 @@ TEST_F(EkfMagTest, fusionStartWithReset) 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); + EXPECT_NEAR(degrees(mag_decl), mag_decl_wmm_deg, 1e-5f); float mag_incl = asinf(mag_earth(2) / fmaxf(mag_earth.norm(), 1e-4f)); float mag_incl_wmm_deg = 0.f; diff --git a/src/modules/ekf2/test/test_EKF_yaw_fusion_generated.cpp b/src/modules/ekf2/test/test_EKF_yaw_fusion_generated.cpp index ab02fd3cacf8..7e536e62bb79 100644 --- a/src/modules/ekf2/test/test_EKF_yaw_fusion_generated.cpp +++ b/src/modules/ekf2/test/test_EKF_yaw_fusion_generated.cpp @@ -35,60 +35,16 @@ #include "EKF/ekf.h" #include "test_helper/comparison_helper.h" -#include "../EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h.h" -#include "../EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h_alternate.h" -#include "../EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h.h" -#include "../EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h_alternate.h" +#include "../EKF/python/ekf_derivation/generated/compute_yaw_innov_var_and_h.h" using namespace matrix; -TEST(YawFusionGenerated, singularityYawEquivalence) +Vector3f getRotVarNed(const Quatf &q, const SquareMatrixState &P) { - // GIVEN: an attitude that should give a singularity when transforming the - // rotation matrix to Euler yaw - StateSample state{}; - state.quat_nominal = Eulerf(M_PI_F, 0.f, M_PI_F); - - const float R = sq(radians(10.f)); - SquareMatrixState P = createRandomCovarianceMatrix(); - - VectorState H_a; - VectorState H_b; - float innov_var_a; - float innov_var_b; - - // WHEN: computing the innovation variance and H using two different - // alternate forms (one is singular at pi/2 and the other one at 0) - sym::ComputeYaw321InnovVarAndH(state.vector(), P, R, FLT_EPSILON, &innov_var_a, &H_a); - sym::ComputeYaw321InnovVarAndHAlternate(state.vector(), P, R, FLT_EPSILON, &innov_var_b, &H_b); - - // THEN: Even at the singularity point, the result is still correct, thanks to epsilon - EXPECT_TRUE(isEqual(H_a, H_b)); - EXPECT_NEAR(innov_var_a, innov_var_b, 1e-5f); - EXPECT_TRUE(innov_var_a < 50.f && innov_var_a > R) << "innov_var = " << innov_var_a; -} - -TEST(YawFusionGenerated, gimbalLock321vs312) -{ - // GIVEN: an attitude at gimbal lock position - StateSample state{}; - state.quat_nominal = Eulerf(0.f, -M_PI_F / 2.f, M_PI_F); - - const float R = sq(radians(10.f)); - SquareMatrixState P = createRandomCovarianceMatrix(); - - VectorState H_321; - VectorState H_312; - float innov_var_321; - float innov_var_312; - sym::ComputeYaw321InnovVarAndH(state.vector(), P, R, FLT_EPSILON, &innov_var_321, &H_321); - - sym::ComputeYaw312InnovVarAndH(state.vector(), P, R, FLT_EPSILON, &innov_var_312, &H_312); - - // THEN: both computation are not equivalent, 321 is undefined but 312 is valid - EXPECT_FALSE(isEqual(H_321, H_312)); - EXPECT_GT(fabsf(innov_var_321 - innov_var_312), 1e6f); - EXPECT_TRUE(innov_var_312 < 50.f && innov_var_312 > R) << "innov_var = " << innov_var_312; + constexpr auto S = State::quat_nominal; + matrix::SquareMatrix3f rot_cov_body = P.slice(S.idx, S.idx); + auto R_to_earth = Dcmf(q); + return matrix::SquareMatrix(R_to_earth * rot_cov_body * R_to_earth.T()).diag(); } TEST(YawFusionGenerated, positiveVarianceAllOrientations) @@ -99,19 +55,14 @@ TEST(YawFusionGenerated, positiveVarianceAllOrientations) VectorState H; float innov_var; - // GIVEN: all orientations (90 deg steps) - for (float yaw = 0.f; yaw < 2.f * M_PI_F; yaw += M_PI_F / 2.f) { - for (float pitch = 0.f; pitch < 2.f * M_PI_F; pitch += M_PI_F / 2.f) { - for (float roll = 0.f; roll < 2.f * M_PI_F; roll += M_PI_F / 2.f) { + // GIVEN: all orientations + for (float yaw = 0.f; yaw < 2.f * M_PI_F; yaw += M_PI_F / 4.f) { + for (float pitch = 0.f; pitch < 2.f * M_PI_F; pitch += M_PI_F / 4.f) { + for (float roll = 0.f; roll < 2.f * M_PI_F; roll += M_PI_F / 4.f) { StateSample state{}; state.quat_nominal = Eulerf(roll, pitch, yaw); - if (shouldUse321RotationSequence(Dcmf(state.quat_nominal))) { - sym::ComputeYaw321InnovVarAndH(state.vector(), P, R, FLT_EPSILON, &innov_var, &H); - - } else { - sym::ComputeYaw312InnovVarAndH(state.vector(), P, R, FLT_EPSILON, &innov_var, &H); - } + sym::ComputeYawInnovVarAndH(state.vector(), P, R, &innov_var, &H); // THEN: the innovation variance must be positive and finite EXPECT_TRUE(innov_var < 100.f && innov_var > R) @@ -119,6 +70,18 @@ TEST(YawFusionGenerated, positiveVarianceAllOrientations) << " pitch = " << degrees(pitch) << " roll = " << degrees(roll) << " innov_var = " << innov_var; + + // AND: it should be the same as the "true" innovation variance obtained by summing + // the Z rotation variance in NED and the measurement variance + const float innov_var_true = getRotVarNed(state.quat_nominal, P)(2) + R; + EXPECT_NEAR(innov_var, innov_var_true, 1e-5f) + << "yaw = " << degrees(yaw) + << " pitch = " << degrees(pitch) + << " roll = " << degrees(roll) + << " innov_var = " << innov_var + << " innov_var_true = " << innov_var_true; + + EXPECT_TRUE(H.isAllFinite()); } } } diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index 1c7b4d82afeb..4c5fbc8be785 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -281,6 +281,12 @@ void FlightTaskAuto::_prepareLandSetpoints() sticks_xy.setZero(); } + // If ground distance estimate valid (distance sensor) during nudging then limit horizontal speed + if (PX4_ISFINITE(_dist_to_bottom)) { + // Below 50cm no horizontal speed, above allow per meter altitude 0.5m/s speed + max_speed = math::max(0.f, math::min(max_speed, (_dist_to_bottom - .5f) * .5f)); + } + _stick_acceleration_xy.setVelocityConstraint(max_speed); _stick_acceleration_xy.generateSetpoints(sticks_xy, _yaw, _land_heading, _position, _velocity_setpoint_feedback.xy(), _deltatime); @@ -803,8 +809,8 @@ void FlightTaskAuto::_updateTrajConstraints() if (_is_emergency_braking_active) { // When initializing with large velocity, allow 1g of // acceleration in 1s on all axes for fast braking - _position_smoothing.setMaxAcceleration({9.81f, 9.81f, 9.81f}); - _position_smoothing.setMaxJerk(9.81f); + _position_smoothing.setMaxAcceleration({CONSTANTS_ONE_G, CONSTANTS_ONE_G, CONSTANTS_ONE_G}); + _position_smoothing.setMaxJerk(CONSTANTS_ONE_G); // If the current velocity is beyond the usual constraints, tell // the controller to exceptionally increase its saturations to avoid diff --git a/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt index 62b8e1217915..911da11d2682 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt +++ b/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt @@ -40,3 +40,5 @@ px4_add_library(FlightTaskUtility target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis bezier SlewRate motion_planning mathlib) target_include_directories(FlightTaskUtility PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +px4_add_functional_gtest(SRC StickTiltXYTest.cpp LINKLIBS FlightTaskUtility) diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickTiltXY.cpp b/src/modules/flight_mode_manager/tasks/Utility/StickTiltXY.cpp index 49c9b2f83cc0..642c4ef8a196 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickTiltXY.cpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickTiltXY.cpp @@ -40,7 +40,18 @@ using namespace matrix; StickTiltXY::StickTiltXY(ModuleParams *parent) : ModuleParams(parent) -{} +{ + updateParams(); +} + +void StickTiltXY::updateParams() +{ + ModuleParams::updateParams(); + // Consider maximum tilt but only between [0.02,3]g sideways acceleration -> ~[1,71]° tilt + // Constrain tilt already because tanf(90+°) will give negative result + const float maximum_tilt = math::radians(math::constrain(_param_mpc_man_tilt_max.get(), 0.f, 89.f)); + _maximum_acceleration = math::constrain(tanf(maximum_tilt), .02f, 3.f) * CONSTANTS_ONE_G; +} Vector2f StickTiltXY::generateAccelerationSetpoints(Vector2f stick_xy, const float dt, const float yaw, const float yaw_setpoint) @@ -49,5 +60,5 @@ Vector2f StickTiltXY::generateAccelerationSetpoints(Vector2f stick_xy, const flo _man_input_filter.setParameters(dt, _param_mc_man_tilt_tau.get()); stick_xy = _man_input_filter.update(stick_xy); Sticks::rotateIntoHeadingFrameXY(stick_xy, yaw, yaw_setpoint); - return stick_xy * tanf(math::radians(_param_mpc_man_tilt_max.get())) * CONSTANTS_ONE_G; + return stick_xy * _maximum_acceleration; } diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickTiltXY.hpp b/src/modules/flight_mode_manager/tasks/Utility/StickTiltXY.hpp index 38f42ed3849e..f5caea855066 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickTiltXY.hpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickTiltXY.hpp @@ -63,6 +63,9 @@ class StickTiltXY : public ModuleParams matrix::Vector2f generateAccelerationSetpoints(matrix::Vector2f stick_xy, const float dt, const float yaw, const float yaw_setpoint); private: + void updateParams() override; + + float _maximum_acceleration{0.f}; AlphaFilter _man_input_filter; DEFINE_PARAMETERS( diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickTiltXYTest.cpp b/src/modules/flight_mode_manager/tasks/Utility/StickTiltXYTest.cpp new file mode 100644 index 000000000000..0f7f88c3cc59 --- /dev/null +++ b/src/modules/flight_mode_manager/tasks/Utility/StickTiltXYTest.cpp @@ -0,0 +1,102 @@ +/**************************************************************************** + * + * Copyright (C) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include "StickTiltXY.hpp" + +#include + +using namespace matrix; + +TEST(StickTiltXYTest, AllZeroCase) +{ + StickTiltXY stick_tilt_xy{nullptr}; + Vector2f acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(), 0.f, 0.f, 0.f); + EXPECT_EQ(acc_xy, Vector2f()); +} + +TEST(StickTiltXYTest, NormalRollPitchCases) +{ + // Disable autosaving parameters to avoid busy loop in param_set() + param_control_autosave(false); + + float value = 45.f; + param_set(param_find("MPC_MAN_TILT_MAX"), &value); + + StickTiltXY stick_tilt_xy{nullptr}; + // Pitch + Vector2f acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(1.f, 0.f), 1.f, 0.f, 0.f); + EXPECT_EQ(acc_xy, Vector2f(CONSTANTS_ONE_G, 0.f)); + acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(.5f, 0.f), 1.f, 0.f, 0.f); + EXPECT_EQ(acc_xy, Vector2f(CONSTANTS_ONE_G / 2.f, 0.f)); + acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(-.5f, 0.f), 1.f, 0.f, 0.f); + EXPECT_EQ(acc_xy, Vector2f(-CONSTANTS_ONE_G / 2.f, 0.f)); + acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(-1.f, 0.f), 1.f, 0.f, 0.f); + EXPECT_EQ(acc_xy, Vector2f(-CONSTANTS_ONE_G, 0.f)); + // Roll + acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(0.f, 1.f), 1.f, 0.f, 0.f); + EXPECT_EQ(acc_xy, Vector2f(0.f, CONSTANTS_ONE_G)); + acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(0.f, .5f), 1.f, 0.f, 0.f); + EXPECT_EQ(acc_xy, Vector2f(0.f, CONSTANTS_ONE_G / 2.f)); + acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(0.f, -.5f), 1.f, 0.f, 0.f); + EXPECT_EQ(acc_xy, Vector2f(0.f, -CONSTANTS_ONE_G / 2.f)); + acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(0.f, -1.f), 1.f, 0.f, 0.f); + EXPECT_EQ(acc_xy, Vector2f(0.f, -CONSTANTS_ONE_G)); + // Roll & Pitch + acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(1.f, 1.f), 1.f, 0.f, 0.f); + EXPECT_EQ(acc_xy, Vector2f(CONSTANTS_ONE_G / M_SQRT2_F, CONSTANTS_ONE_G / M_SQRT2_F)); + acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(1.f, -1.f), 1.f, 0.f, 0.f); + EXPECT_EQ(acc_xy, Vector2f(CONSTANTS_ONE_G / M_SQRT2_F, -CONSTANTS_ONE_G / M_SQRT2_F)); + acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(-1.f, 1.f), 1.f, 0.f, 0.f); + EXPECT_EQ(acc_xy, Vector2f(-CONSTANTS_ONE_G / M_SQRT2_F, CONSTANTS_ONE_G / M_SQRT2_F)); + acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(-1.f, -1.f), 1.f, 0.f, 0.f); + EXPECT_EQ(acc_xy, Vector2f(-CONSTANTS_ONE_G / M_SQRT2_F, -CONSTANTS_ONE_G / M_SQRT2_F)); +} + +TEST(StickTiltXYTest, 90degreeCase) +{ + // Disable autosaving parameters to avoid busy loop in param_set() + param_control_autosave(false); + + float value = 90.f; + param_set(param_find("MPC_MAN_TILT_MAX"), &value); + + StickTiltXY stick_tilt_xy{nullptr}; + // Pitch + // Zero input leads to zero output + Vector2f acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(), 1.f, 0.f, 0.f); + EXPECT_EQ(acc_xy, Vector2f()); + // Maximum input leads to the maximum of 3g sideways acceleration + acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(1.f, 0.f), 1.f, 0.f, 0.f); + EXPECT_EQ(acc_xy, Vector2f(3.f * CONSTANTS_ONE_G, 0.f)); +} diff --git a/src/modules/flight_mode_manager/tasks/Utility/Sticks.hpp b/src/modules/flight_mode_manager/tasks/Utility/Sticks.hpp index be8b69e306c9..3ab13368e841 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/Sticks.hpp +++ b/src/modules/flight_mode_manager/tasks/Utility/Sticks.hpp @@ -92,7 +92,7 @@ class Sticks : public ModuleParams private: bool _input_available{false}; - matrix::Vector4f _positions; ///< unmodified manual stick inputs + matrix::Vector4f _positions; ///< unmodified manual stick inputs that usually move vehicle in x, y, z and yaw direction matrix::Vector4f _positions_expo; ///< modified manual sticks using expo function matrix::Vector _aux_positions; diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 35632b7aec60..7de353c8f977 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -361,7 +361,7 @@ void FixedwingAttitudeControl::Run() /* add yaw rate setpoint from sticks in all attitude-controlled modes */ if (_vcontrol_mode.flag_control_manual_enabled) { - body_rates_setpoint(2) += math::constrain(_manual_control_setpoint.yaw * radians(_param_fw_y_rmax.get()), + body_rates_setpoint(2) += math::constrain(_manual_control_setpoint.yaw * radians(_param_man_yr_max.get()), -radians(_param_fw_y_rmax.get()), radians(_param_fw_y_rmax.get())); } diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index 20421f056dd5..6c902878e1f6 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -155,7 +155,8 @@ class FixedwingAttitudeControl final : public ModuleBase) _param_fw_wr_imax, (ParamFloat) _param_fw_wr_p, - (ParamFloat) _param_fw_y_rmax + (ParamFloat) _param_fw_y_rmax, + (ParamFloat) _param_man_yr_max ) diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index b210b01b6688..4bb1d5ee3188 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -129,8 +129,8 @@ FixedwingPositionControl::parameters_update() _tecs.set_speed_weight(_param_fw_t_spdweight.get()); _tecs.set_equivalent_airspeed_trim(_performance_model.getCalibratedTrimAirspeed()); _tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed()); - _tecs.set_throttle_damp(_param_fw_t_thr_damp.get()); - _tecs.set_integrator_gain_throttle(_param_fw_t_I_gain_thr.get()); + _tecs.set_throttle_damp(_param_fw_t_thr_damping.get()); + _tecs.set_integrator_gain_throttle(_param_fw_t_thr_integ.get()); _tecs.set_integrator_gain_pitch(_param_fw_t_I_gain_pit.get()); _tecs.set_throttle_slewrate(_param_fw_thr_slew_max.get()); _tecs.set_vertical_accel_limit(_param_fw_t_vert_acc.get()); @@ -510,18 +510,40 @@ float FixedwingPositionControl::getCorrectedNpfgRollSetpoint() { // Scale the npfg output to zero if npfg is not certain for correct output float new_roll_setpoint(_npfg.getRollSetpoint()); - const float can_run_factor(_npfg.canRun(_local_pos, _wind_valid)); + const float can_run_factor(constrain(_npfg.canRun(_local_pos, _wind_valid), 0.f, 1.f)); - if ((1.f - can_run_factor) < FLT_EPSILON) { + hrt_abstime now{hrt_absolute_time()}; + + // Warn the user when the scale is less than 90% for at least 2 seconds (disable in transition) + + // If the npfg was not running before, reset the user warning variables. + if ((now - _time_since_last_npfg_call) > ROLL_WARNING_TIMEOUT) { _need_report_npfg_uncertain_condition = true; + _time_since_first_reduced_roll = 0U; } - if (((1.f - can_run_factor) > FLT_EPSILON) && _need_report_npfg_uncertain_condition) { - _need_report_npfg_uncertain_condition = false; - events::send(events::ID("npfg_roll_command_uncertain"), events::Log::Warning, - "Roll command reduced due to uncertain velocity/wind estimates!"); + if (_vehicle_status.in_transition_mode || can_run_factor > ROLL_WARNING_CAN_RUN_THRESHOLD || _landed) { + // NPFG reports a good condition or we are in transition, reset the user warning variables. + _need_report_npfg_uncertain_condition = true; + _time_since_first_reduced_roll = 0U; + + } else if (_need_report_npfg_uncertain_condition) { + if (_time_since_first_reduced_roll == 0U) { + _time_since_first_reduced_roll = now; + } + + if ((now - _time_since_first_reduced_roll) > ROLL_WARNING_TIMEOUT) { + _need_report_npfg_uncertain_condition = false; + events::send(events::ID("npfg_roll_command_uncertain"), events::Log::Warning, + "Roll command reduced due to uncertain velocity/wind estimates!"); + } + + } else { + // Nothing to do, already reported. } + _time_since_last_npfg_call = now; + return can_run_factor * (new_roll_setpoint); } @@ -666,14 +688,32 @@ 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 - || _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE)) { + if (_control_mode.flag_control_offboard_enabled && _position_setpoint_current_valid + && _control_mode.flag_control_position_enabled) { + if (PX4_ISFINITE(_pos_sp_triplet.current.vx) && PX4_ISFINITE(_pos_sp_triplet.current.vy) + && PX4_ISFINITE(_pos_sp_triplet.current.vz)) { + // Offboard position with velocity setpoints + _control_mode_current = FW_POSCTRL_MODE_AUTO_PATH; + return; + + } else { + // Offboard position setpoint only + _control_mode_current = FW_POSCTRL_MODE_AUTO; + return; + } + + } else if ((_control_mode.flag_control_auto_enabled && _control_mode.flag_control_position_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. + const bool doing_backtransition = _vehicle_status.in_transition_mode && !_vehicle_status.in_transition_to_fw; + + if (doing_backtransition) { + _control_mode_current = FW_POSCTRL_MODE_TRANSITON; - if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { + } else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { if (_vehicle_status.is_vtol && _vehicle_status.in_transition_mode) { _control_mode_current = FW_POSCTRL_MODE_AUTO; @@ -694,22 +734,14 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now) } else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { - if (!_vehicle_status.in_transition_mode) { - - // Use _position_setpoint_previous_valid to determine if landing should be straight or circular. - // Straight landings are currently only possible in Missions, and there the previous WP - // is valid, and circular ones are used outside of Missions, as the land mode sets prev_valid=false. - if (_position_setpoint_previous_valid) { - _control_mode_current = FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT; - - } else { - _control_mode_current = FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR; - } + // Use _position_setpoint_previous_valid to determine if landing should be straight or circular. + // Straight landings are currently only possible in Missions, and there the previous WP + // is valid, and circular ones are used outside of Missions, as the land mode sets prev_valid=false. + if (_position_setpoint_previous_valid) { + _control_mode_current = FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT; } else { - _control_mode_current = FW_POSCTRL_MODE_AUTO; - // in this case we want the waypoint handled as a position setpoint -- a submode in control_auto() - _pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; + _control_mode_current = FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR; } } else { @@ -824,7 +856,7 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto position_setpoint_s current_sp = pos_sp_curr; move_position_setpoint_for_vtol_transition(current_sp); - const uint8_t position_sp_type = handle_setpoint_type(current_sp); + const uint8_t position_sp_type = handle_setpoint_type(current_sp, pos_sp_next); _position_sp_type = position_sp_type; @@ -957,7 +989,8 @@ FixedwingPositionControl::control_auto_descend(const float control_interval) } uint8_t -FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp_curr) +FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp_curr, + const position_setpoint_s &pos_sp_next) { uint8_t position_sp_type = pos_sp_curr.type; @@ -972,8 +1005,13 @@ FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp const float acc_rad = _npfg.switchDistance(500.0f); - if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION - || pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) { + const bool approaching_vtol_backtransition = _vehicle_status.is_vtol + && pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION && _position_setpoint_current_valid + && pos_sp_next.type == position_setpoint_s::SETPOINT_TYPE_LAND && _position_setpoint_next_valid; + + + // check if we should switch to loiter but only if we are not expecting a backtransition to happen + if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION && !approaching_vtol_backtransition) { float dist_xy = -1.f; float dist_z = -1.f; @@ -983,17 +1021,15 @@ FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp _current_latitude, _current_longitude, _current_altitude, &dist_xy, &dist_z); - if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { - // Achieve position setpoint altitude via loiter when laterally close to WP. - // Detect if system has switchted into a Loiter before (check _position_sp_type), and in that - // case remove the dist_xy check (not switch out of Loiter until altitude is reached). - if ((!_vehicle_status.in_transition_mode) && (dist >= 0.f) - && (dist_z > _param_nav_fw_alt_rad.get()) - && (dist_xy < acc_rad || _position_sp_type == position_setpoint_s::SETPOINT_TYPE_LOITER)) { + // Achieve position setpoint altitude via loiter when laterally close to WP. + // Detect if system has switchted into a Loiter before (check _position_sp_type), and in that + // case remove the dist_xy check (not switch out of Loiter until altitude is reached). + if ((!_vehicle_status.in_transition_mode) && (dist >= 0.f) + && (dist_z > _param_nav_fw_alt_rad.get()) + && (dist_xy < acc_rad || _position_sp_type == position_setpoint_s::SETPOINT_TYPE_LOITER)) { - // SETPOINT_TYPE_POSITION -> SETPOINT_TYPE_LOITER - position_sp_type = position_setpoint_s::SETPOINT_TYPE_LOITER; - } + // SETPOINT_TYPE_POSITION -> SETPOINT_TYPE_LOITER + position_sp_type = position_setpoint_s::SETPOINT_TYPE_LOITER; } } @@ -1063,15 +1099,7 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co _npfg.setAirspeedNom(target_airspeed * _eas2tas); _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); - if (_control_mode.flag_control_offboard_enabled && PX4_ISFINITE(pos_sp_curr.vx) && PX4_ISFINITE(pos_sp_curr.vy)) { - // Navigate directly on position setpoint and path tangent - matrix::Vector2f velocity_2d(pos_sp_curr.vx, pos_sp_curr.vy); - float curvature = PX4_ISFINITE(_pos_sp_triplet.current.loiter_radius) ? 1 / _pos_sp_triplet.current.loiter_radius : - 0.0f; - navigatePathTangent(curr_pos_local, curr_wp_local, velocity_2d.normalized(), ground_speed, - _wind_vel, curvature); - - } else if (_position_setpoint_previous_valid && pos_sp_prev.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { + 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); @@ -1326,6 +1354,58 @@ void FixedwingPositionControl::publishFigureEightStatus(const position_setpoint_ } #endif // CONFIG_FIGURE_OF_EIGHT +void +FixedwingPositionControl::control_auto_path(const float control_interval, const Vector2d &curr_pos, + const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr) +{ + + float tecs_fw_thr_min; + float tecs_fw_thr_max; + + if (pos_sp_curr.gliding_enabled) { + /* enable gliding with this waypoint */ + _tecs.set_speed_weight(2.0f); + tecs_fw_thr_min = 0.0; + tecs_fw_thr_max = 0.0; + + } else { + tecs_fw_thr_min = _param_fw_thr_min.get(); + tecs_fw_thr_max = _param_fw_thr_max.get(); + } + + // waypoint is a plain navigation waypoint + float target_airspeed = adapt_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed, + _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()), ground_speed); + + Vector2f curr_pos_local{_local_pos.x, _local_pos.y}; + Vector2f curr_wp_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon); + + _npfg.setAirspeedNom(target_airspeed * _eas2tas); + _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); + + // Navigate directly on position setpoint and path tangent + matrix::Vector2f velocity_2d(pos_sp_curr.vx, pos_sp_curr.vy); + const float curvature = PX4_ISFINITE(_pos_sp_triplet.current.loiter_radius) ? 1 / + _pos_sp_triplet.current.loiter_radius : + 0.0f; + navigatePathTangent(curr_pos_local, curr_wp_local, velocity_2d.normalized(), ground_speed, _wind_vel, curvature); + + _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + target_airspeed = _npfg.getAirspeedRef() / _eas2tas; + + _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + + tecs_update_pitch_throttle(control_interval, + pos_sp_curr.alt, + target_airspeed, + radians(_param_fw_p_lim_min.get()), + radians(_param_fw_p_lim_max.get()), + tecs_fw_thr_min, + tecs_fw_thr_max, + _param_sinkrate_target.get(), + _param_climbrate_target.get()); +} + void FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const float control_interval, const Vector2d &global_position, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr) @@ -2120,6 +2200,44 @@ FixedwingPositionControl::control_manual_position(const float control_interval, _att_sp.pitch_body = get_tecs_pitch(); } +void FixedwingPositionControl::control_backtransition(const float control_interval, const Vector2f &ground_speed, + const position_setpoint_s &pos_sp_curr) +{ + float target_airspeed = adapt_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed, + _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()), ground_speed); + + Vector2f curr_pos_local{_local_pos.x, _local_pos.y}; + Vector2f curr_wp_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon); + + _npfg.setAirspeedNom(target_airspeed * _eas2tas); + _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); + + // Set the position where the backtransition started the first ime we pass through here. + // Will get reset if not in transition anymore. + if (!_lpos_where_backtrans_started.isAllFinite()) { + _lpos_where_backtrans_started = curr_pos_local; + } + + navigateLine(_lpos_where_backtrans_started, curr_wp_local, curr_pos_local, ground_speed, _wind_vel); + + _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + target_airspeed = _npfg.getAirspeedRef() / _eas2tas; + + _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + + tecs_update_pitch_throttle(control_interval, + pos_sp_curr.alt, + target_airspeed, + radians(_param_fw_p_lim_min.get()), + radians(_param_fw_p_lim_max.get()), + _param_fw_thr_min.get(), + _param_fw_thr_max.get(), + _param_sinkrate_target.get(), + _param_climbrate_target.get()); + + _att_sp.thrust_body[0] = (_landed) ? _param_fw_thr_min.get() : min(get_tecs_thrust(), _param_fw_thr_max.get()); + _att_sp.pitch_body = get_tecs_pitch(); +} float FixedwingPositionControl::get_tecs_pitch() { @@ -2323,7 +2441,12 @@ FixedwingPositionControl::Run() } } - _vehicle_status_sub.update(&_vehicle_status); + if (_vehicle_status_sub.update(&_vehicle_status)) { + if (!_vehicle_status.in_transition_mode) { + // reset position of backtransition start if not in transition + _lpos_where_backtrans_started = Vector2f(NAN, NAN); + } + } Vector2d curr_pos(_current_latitude, _current_longitude); Vector2f ground_speed(_local_pos.vx, _local_pos.vy); @@ -2345,6 +2468,9 @@ FixedwingPositionControl::Run() _flaps_setpoint = 0.f; _spoilers_setpoint = 0.f; + // reset flight phase estimate + _flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_UNKNOWN; + // by default we don't want yaw to be contoller directly with rudder _att_sp.fw_control_yaw_wheel = false; @@ -2391,6 +2517,11 @@ FixedwingPositionControl::Run() break; } + case FW_POSCTRL_MODE_AUTO_PATH: { + control_auto_path(control_interval, curr_pos, ground_speed, _pos_sp_triplet.current); + break; + } + case FW_POSCTRL_MODE_AUTO_TAKEOFF: { control_auto_takeoff(_local_pos.timestamp, control_interval, curr_pos, ground_speed, _pos_sp_triplet.current); break; @@ -2411,8 +2542,13 @@ FixedwingPositionControl::Run() break; } + case FW_POSCTRL_MODE_TRANSITON: { + control_backtransition(control_interval, ground_speed, _pos_sp_triplet.current); + break; + } } + if (_control_mode_current != FW_POSCTRL_MODE_OTHER) { if (_control_mode.flag_control_manual_enabled) { @@ -2449,6 +2585,10 @@ FixedwingPositionControl::Run() _roll_slew_rate.setForcedValue(_roll); } + // Publish estimate of level flight + _flight_phase_estimation_pub.get().timestamp = hrt_absolute_time(); + _flight_phase_estimation_pub.update(); + // if there's any change in landing gear setpoint publish it if (_new_landing_gear_position != old_landing_gear_position && _new_landing_gear_position != landing_gear_s::GEAR_KEEP) { @@ -2552,6 +2692,28 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva hgt_rate_sp); tecs_status_publish(alt_sp, airspeed_sp, airspeed_rate_estimate, throttle_trim_compensated); + + if (_tecs_is_running && !_vehicle_status.in_transition_mode + && (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)) { + const TECS::DebugOutput &tecs_output{_tecs.getStatus()}; + + // Check level flight: the height rate setpoint is not set or set to 0 and we are close to the target altitude and target altitude is not moving + if ((fabsf(tecs_output.height_rate_reference) < MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT) && + fabsf(_current_altitude - tecs_output.altitude_reference) < _param_nav_fw_alt_rad.get()) { + _flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_LEVEL; + + } else if (((tecs_output.altitude_reference - _current_altitude) >= _param_nav_fw_alt_rad.get()) || + (tecs_output.height_rate_reference >= MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT)) { + _flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_CLIMB; + + } else if (((_current_altitude - tecs_output.altitude_reference) >= _param_nav_fw_alt_rad.get()) || + (tecs_output.height_rate_reference <= -MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT)) { + _flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_DESCEND; + + } else { + //We can't infer the flight phase , do nothing, estimation is reset at each step + } + } } float @@ -2999,6 +3161,7 @@ float FixedwingPositionControl::getLoadFactor() } + extern "C" __EXPORT int fw_pos_control_main(int argc, char *argv[]) { return FixedwingPositionControl::main(argc, argv); diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp index 16c857e35bda..5089021685da 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.hpp @@ -72,6 +72,7 @@ #include #include #include +#include #include #include #include @@ -101,6 +102,7 @@ #ifdef CONFIG_FIGURE_OF_EIGHT #include "figure_eight/FigureEight.hpp" #include + #endif // CONFIG_FIGURE_OF_EIGHT using namespace launchdetection; @@ -164,6 +166,16 @@ static constexpr float MANUAL_TOUCHDOWN_NUDGE_INPUT_DEADZONE = 0.15f; // [s] time interval after touchdown for ramping in runway clamping constraints (touchdown is assumed at FW_LND_TD_TIME after start of flare) static constexpr float POST_TOUCHDOWN_CLAMP_TIME = 0.5f; +// [m/s] maximum reference altitude rate threshhold +static constexpr float MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT = 0.1f; + +// [s] Timeout that has to pass in roll-constraining failsafe before warning is triggered +static constexpr uint64_t ROLL_WARNING_TIMEOUT = 2_s; + +// [-] Can-run threshold needed to trigger the roll-constraining failsafe warning +static constexpr float ROLL_WARNING_CAN_RUN_THRESHOLD = 0.9f; + + class FixedwingPositionControl final : public ModuleBase, public ModuleParams, public px4::WorkItem { @@ -214,6 +226,7 @@ class FixedwingPositionControl final : public ModuleBase _landing_gear_pub {ORB_ID(landing_gear)}; uORB::Publication _flaps_setpoint_pub{ORB_ID(flaps_setpoint)}; uORB::Publication _spoilers_setpoint_pub{ORB_ID(spoilers_setpoint)}; + uORB::PublicationData _flight_phase_estimation_pub{ORB_ID(flight_phase_estimation)}; manual_control_setpoint_s _manual_control_setpoint{}; position_setpoint_triplet_s _pos_sp_triplet{}; @@ -222,6 +235,8 @@ class FixedwingPositionControl final : public ModuleBase) _param_fw_t_hrate_ff, (ParamFloat) _param_fw_t_h_error_tc, - (ParamFloat) _param_fw_t_I_gain_thr, + (ParamFloat) _param_fw_t_thr_integ, (ParamFloat) _param_fw_t_I_gain_pit, (ParamFloat) _param_fw_t_ptch_damp, (ParamFloat) _param_fw_t_rll2thr, (ParamFloat) _param_fw_t_sink_max, (ParamFloat) _param_fw_t_spdweight, (ParamFloat) _param_fw_t_tas_error_tc, - (ParamFloat) _param_fw_t_thr_damp, + (ParamFloat) _param_fw_t_thr_damping, (ParamFloat) _param_fw_t_vert_acc, (ParamFloat) _param_ste_rate_time_const, (ParamFloat) _param_seb_rate_ff, 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 07518de71087..a636d8d75768 100644 --- a/src/modules/fw_pos_control/fw_path_navigation_params.c +++ b/src/modules/fw_pos_control/fw_path_navigation_params.c @@ -465,38 +465,34 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); * Increase to add damping to correct for oscillations in speed and height. * * @min 0.0 - * @max 2.0 - * @decimal 2 - * @increment 0.1 + * @max 1.0 + * @decimal 3 + * @increment 0.01 * @group FW TECS */ -PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.1f); +PARAM_DEFINE_FLOAT(FW_T_THR_DAMPING, 0.05f); /** * Integrator gain throttle * - * This is the integrator gain on the throttle part of the control loop. - * Increasing this gain increases the speed at which speed - * and height offsets are trimmed out, but reduces damping and - * increases overshoot. Set this value to zero to completely - * disable all integrator action. + * Integrator gain on the throttle part of the control loop. + * Increase it to trim out speed and height offsets faster, + * with the downside of possible overshoots and oscillations. * * @min 0.0 - * @max 2.0 - * @decimal 2 - * @increment 0.05 + * @max 1.0 + * @decimal 3 + * @increment 0.005 * @group FW TECS */ -PARAM_DEFINE_FLOAT(FW_T_I_GAIN_THR, 0.05f); +PARAM_DEFINE_FLOAT(FW_T_THR_INTEG, 0.02f); /** * Integrator gain pitch * - * This is the integrator gain on the pitch part of the control loop. - * Increasing this gain increases the speed at which speed - * and height offsets are trimmed out, but reduces damping and - * increases overshoot. Set this value to zero to completely - * disable all integrator action. + * Integrator gain on the pitch part of the control loop. + * Increase it to trim out speed and height offsets faster, + * with the downside of possible overshoots and oscillations. * * @min 0.0 * @max 2.0 diff --git a/src/modules/fw_rate_control/FixedwingRateControl.cpp b/src/modules/fw_rate_control/FixedwingRateControl.cpp index 37c2d98ae035..1f2cff00d6cd 100644 --- a/src/modules/fw_rate_control/FixedwingRateControl.cpp +++ b/src/modules/fw_rate_control/FixedwingRateControl.cpp @@ -322,28 +322,26 @@ void FixedwingRateControl::Run() } /* bi-linear interpolation over airspeed for actuator trim scheduling */ - float trim_roll = _param_trim_roll.get(); - float trim_pitch = _param_trim_pitch.get(); - float trim_yaw = _param_trim_yaw.get(); + Vector3f trim(_param_trim_roll.get(), _param_trim_pitch.get(), _param_trim_yaw.get()); if (airspeed < _param_fw_airspd_trim.get()) { - trim_roll += interpolate(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), - _param_fw_dtrim_r_vmin.get(), - 0.0f); - trim_pitch += interpolate(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), - _param_fw_dtrim_p_vmin.get(), - 0.0f); - trim_yaw += interpolate(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), - _param_fw_dtrim_y_vmin.get(), - 0.0f); + trim(0) += interpolate(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), + _param_fw_dtrim_r_vmin.get(), + 0.0f); + trim(1) += interpolate(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), + _param_fw_dtrim_p_vmin.get(), + 0.0f); + trim(2) += interpolate(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), + _param_fw_dtrim_y_vmin.get(), + 0.0f); } else { - trim_roll += interpolate(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f, - _param_fw_dtrim_r_vmax.get()); - trim_pitch += interpolate(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f, - _param_fw_dtrim_p_vmax.get()); - trim_yaw += interpolate(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f, - _param_fw_dtrim_y_vmax.get()); + trim(0) += interpolate(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f, + _param_fw_dtrim_r_vmax.get()); + trim(1) += interpolate(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f, + _param_fw_dtrim_p_vmax.get()); + trim(2) += interpolate(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f, + _param_fw_dtrim_y_vmax.get()); } if (_vcontrol_mode.flag_control_rates_enabled) { @@ -360,32 +358,25 @@ void FixedwingRateControl::Run() const Vector3f angular_acceleration_setpoint = _rate_control.update(rates, body_rates_setpoint, angular_accel, dt, _landed); - const float roll_feedforward = _param_fw_rr_ff.get() * _airspeed_scaling * body_rates_setpoint(0); - const float pitch_feedforward = _param_fw_pr_ff.get() * _airspeed_scaling * body_rates_setpoint(1); - const float yaw_feedforward = _param_fw_yr_ff.get() * _airspeed_scaling * body_rates_setpoint(2); + const Vector3f gain_ff(_param_fw_rr_ff.get(), _param_fw_pr_ff.get(), _param_fw_yr_ff.get()); + const Vector3f feedforward = gain_ff.emult(body_rates_setpoint) * _airspeed_scaling; - const float roll_u = angular_acceleration_setpoint(0) * _airspeed_scaling * _airspeed_scaling + roll_feedforward; - const float pitch_u = angular_acceleration_setpoint(1) * _airspeed_scaling * _airspeed_scaling + pitch_feedforward; + Vector3f control_u = angular_acceleration_setpoint * _airspeed_scaling * _airspeed_scaling + feedforward; // Special case yaw in Acro: if the parameter FW_ACRO_YAW_CTL is not set then don't control yaw - float yaw_u = 0.f; - - if (_vcontrol_mode.flag_control_attitude_enabled || _param_fw_acro_yaw_en.get()) { - yaw_u = angular_acceleration_setpoint(2) * _airspeed_scaling * _airspeed_scaling + yaw_feedforward; - - } else { - yaw_u = _manual_control_setpoint.yaw * _param_fw_man_y_sc.get(); + if (!_vcontrol_mode.flag_control_attitude_enabled && !_param_fw_acro_yaw_en.get()) { + control_u(2) = _manual_control_setpoint.yaw * _param_fw_man_y_sc.get(); _rate_control.resetIntegral(2); } - if (!PX4_ISFINITE(roll_u) || !PX4_ISFINITE(pitch_u) || !PX4_ISFINITE(yaw_u)) { + if (control_u.isAllFinite()) { + matrix::constrain(control_u + trim, -1.f, 1.f).copyTo(_vehicle_torque_setpoint.xyz); + + } else { _rate_control.resetIntegral(); + trim.copyTo(_vehicle_torque_setpoint.xyz); } - _vehicle_torque_setpoint.xyz[0] = PX4_ISFINITE(roll_u) ? math::constrain(roll_u + trim_roll, -1.f, 1.f) : trim_roll; - _vehicle_torque_setpoint.xyz[1] = PX4_ISFINITE(pitch_u) ? math::constrain(pitch_u + trim_pitch, -1.f, 1.f) : trim_pitch; - _vehicle_torque_setpoint.xyz[2] = PX4_ISFINITE(yaw_u) ? math::constrain(yaw_u + trim_yaw, -1.f, 1.f) : trim_yaw; - /* throttle passed through if it is finite */ _vehicle_thrust_setpoint.xyz[0] = PX4_ISFINITE(_rates_sp.thrust_body[0]) ? _rates_sp.thrust_body[0] : 0.0f; diff --git a/src/modules/gimbal/gimbal.cpp b/src/modules/gimbal/gimbal.cpp index c663d31cea1b..5893d29738f2 100644 --- a/src/modules/gimbal/gimbal.cpp +++ b/src/modules/gimbal/gimbal.cpp @@ -211,7 +211,10 @@ static int gimbal_thread_main(int argc, char *argv[]) // Reset control as no one is active anymore, or yet. thread_data.control_data.sysid_primary_control = 0; thread_data.control_data.compid_primary_control = 0; - thread_data.control_data.device_compid = 0; + // If the output is set to AUX we still want the input to be able to control the gimbal + // via mavlink, so we set the device_compid to 1. This follows the mavlink spec which states + // that the gimbal_device_id should be between 1 and 6. + thread_data.control_data.device_compid = params.mnt_mode_out == 0 ? 1 : 0; } InputBase::UpdateResult update_result = InputBase::UpdateResult::NoUpdate; diff --git a/src/modules/gimbal/input_mavlink.cpp b/src/modules/gimbal/input_mavlink.cpp index 78cf6ae806f7..ac7cbc4e7fd3 100644 --- a/src/modules/gimbal/input_mavlink.cpp +++ b/src/modules/gimbal/input_mavlink.cpp @@ -323,23 +323,28 @@ InputMavlinkCmdMount::_process_command(ControlData &control_data, const vehicle_ }; for (int i = 0; i < 3; ++i) { + switch (params[i]) { - if (params[i] == 0) { + case 0: control_data.type_data.angle.frames[i] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; + break; - } else if (params[i] == 1) { + case 1: control_data.type_data.angle.frames[i] = ControlData::TypeData::TypeAngle::Frame::AngularRate; + break; - } else if (params[i] == 2) { + case 2: control_data.type_data.angle.frames[i] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame; + break; - } else { + default: // Not supported, fallback to body angle. control_data.type_data.angle.frames[i] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; + break; } } @@ -745,23 +750,28 @@ InputMavlinkGimbalV2::_process_command(ControlData &control_data, const vehicle_ }; for (int i = 0; i < 3; ++i) { + switch (params[i]) { - if (params[i] == 0) { + case 0: control_data.type_data.angle.frames[i] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; + break; - } else if (params[i] == 1) { + case 1: control_data.type_data.angle.frames[i] = ControlData::TypeData::TypeAngle::Frame::AngularRate; + break; - } else if (params[i] == 2) { + case 2: control_data.type_data.angle.frames[i] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame; + break; - } else { + default: // Not supported, fallback to body angle. control_data.type_data.angle.frames[i] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; + break; } } @@ -775,19 +785,22 @@ InputMavlinkGimbalV2::_process_command(ControlData &control_data, const vehicle_ const int param_compid = roundf(vehicle_command.param2); uint8_t new_sysid_primary_control = [&]() { - if (param_sysid >= 0 && param_sysid < 256) { + switch (param_sysid) { + + case 0 ... 255: // Valid new sysid. return (uint8_t) param_sysid; - } else if (param_sysid == -1) { + case -1: // leave unchanged return control_data.sysid_primary_control; - } else if (param_sysid == -2) { + case -2: // set itself return (uint8_t) _parameters.mav_sysid; - } else if (param_sysid == -3) { + case -3: + // release control if in control if (control_data.sysid_primary_control == vehicle_command.source_system) { return (uint8_t) 0; @@ -796,26 +809,28 @@ InputMavlinkGimbalV2::_process_command(ControlData &control_data, const vehicle_ return control_data.sysid_primary_control; } - } else { + default: PX4_WARN("Unknown param1 value for DO_GIMBAL_MANAGER_CONFIGURE"); return control_data.sysid_primary_control; } }(); uint8_t new_compid_primary_control = [&]() { - if (param_compid >= 0 && param_compid < 256) { + switch (param_compid) { + case 0 ... 255: // Valid new compid. return (uint8_t) param_compid; - } else if (param_compid == -1) { + case -1: // leave unchanged return control_data.compid_primary_control; - } else if (param_compid == -2) { + case -2: // set itself return (uint8_t) _parameters.mav_compid; - } else if (param_compid == -3) { + case -3: + // release control if in control if (control_data.compid_primary_control == vehicle_command.source_component) { return (uint8_t) 0; @@ -824,7 +839,7 @@ InputMavlinkGimbalV2::_process_command(ControlData &control_data, const vehicle_ return control_data.compid_primary_control; } - } else { + default: PX4_WARN("Unknown param2 value for DO_GIMBAL_MANAGER_CONFIGURE"); return control_data.compid_primary_control; } diff --git a/src/modules/load_mon/LoadMon.cpp b/src/modules/load_mon/LoadMon.cpp index 80c54f482694..aa400391dc52 100644 --- a/src/modules/load_mon/LoadMon.cpp +++ b/src/modules/load_mon/LoadMon.cpp @@ -229,6 +229,9 @@ void LoadMon::cpuload() struct mallinfo mem = mallinfo(); cpuload.ram_usage = (float)mem.uordblks / mem.arena; cpuload.load = 1.f - interval_idletime / interval; +#elif defined(__PX4_QURT) + cpuload.ram_usage = 0.0f; + cpuload.load = px4muorb_get_cpu_load() / 100.0f; #endif cpuload.timestamp = hrt_absolute_time(); diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 9665ca8f6139..cd72d324a0c9 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -53,10 +53,12 @@ void LoggedTopics::add_default_topics() add_optional_topic("autotune_attitude_control_status", 100); add_optional_topic("camera_capture"); add_optional_topic("camera_trigger"); + add_optional_topic("can_interface_status", 10); add_topic("cellular_status", 200); add_topic("commander_state"); add_topic("config_overrides"); add_topic("cpuload"); + add_optional_topic("differential_drive_control_output", 100); add_optional_topic("differential_drive_setpoint", 100); add_optional_topic("external_ins_attitude"); add_optional_topic("external_ins_global_position"); @@ -68,6 +70,7 @@ void LoggedTopics::add_default_topics() add_optional_topic("follow_target_estimator", 200); add_optional_topic("follow_target_status", 400); add_optional_topic("flaps_setpoint", 1000); + add_optional_topic("flight_phase_estimation", 1000); add_topic("gimbal_manager_set_attitude", 500); add_optional_topic("generator_status"); add_optional_topic("gps_dump"); @@ -100,6 +103,8 @@ void LoggedTopics::add_default_topics() add_optional_topic("px4io_status"); add_topic("radio_status"); add_topic("rtl_time_estimate", 1000); + add_optional_topic("rtl_status", 5000); + add_optional_topic("sensor_airflow", 100); add_topic("sensor_combined"); add_optional_topic("sensor_correction"); add_optional_topic("sensor_gyro_fft", 50); @@ -204,7 +209,7 @@ void LoggedTopics::add_default_topics() // add_optional_topic_multi("estimator_aid_src_optical_flow", 100, MAX_ESTIMATOR_INSTANCES); // add_optional_topic_multi("estimator_aid_src_terrain_optical_flow", 100, MAX_ESTIMATOR_INSTANCES); // add_optional_topic_multi("estimator_aid_src_ev_yaw", 100, MAX_ESTIMATOR_INSTANCES); - //add_optional_topic_multi("estimator_aid_src_aux_global_position", 100, MAX_ESTIMATOR_INSTANCES); + add_optional_topic_multi("estimator_aid_src_aux_global_position", 100, MAX_ESTIMATOR_INSTANCES); // log all raw sensors at minimal rate (at least 1 Hz) add_topic_multi("battery_status", 200, 2); @@ -291,6 +296,7 @@ void LoggedTopics::add_default_topics() add_optional_topic_multi("estimator_aid_src_gnss_pos", 0, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_aid_src_gnss_vel", 0, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_aid_src_gnss_yaw", 0, MAX_ESTIMATOR_INSTANCES); + add_optional_topic_multi("estimator_aid_src_gravity", 0, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_aid_src_mag_heading", 0, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_aid_src_mag", 0, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_aid_src_optical_flow", 0, MAX_ESTIMATOR_INSTANCES); @@ -305,7 +311,7 @@ void LoggedTopics::add_high_rate_topics() { // maximum rate to analyze fast maneuvers (e.g. for racing) add_topic("manual_control_setpoint"); - add_topic("rate_ctrl_status", 20); + add_topic_multi("rate_ctrl_status", 20, 2); add_topic("sensor_combined"); add_topic("vehicle_angular_velocity"); add_topic("vehicle_attitude"); @@ -313,8 +319,9 @@ void LoggedTopics::add_high_rate_topics() add_topic("vehicle_rates_setpoint"); add_topic("actuator_motors"); - add_topic("vehicle_thrust_setpoint"); - add_topic("vehicle_torque_setpoint"); + add_topic("actuator_servos"); + add_topic_multi("vehicle_thrust_setpoint", 0, 2); + add_topic_multi("vehicle_torque_setpoint", 0, 2); } void LoggedTopics::add_debug_topics() diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index ecb05295d6c6..f9c3f66800c9 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -1610,6 +1610,11 @@ void Logger::print_load_callback(void *user) void Logger::initialize_load_output(PrintLoadReason reason) { + // If already in progress, don't try to start again + if (_next_load_print != 0) { + return; + } + init_print_load(&_load); if (reason == PrintLoadReason::Watchdog) { diff --git a/src/modules/mag_bias_estimator/MagBiasEstimator.cpp b/src/modules/mag_bias_estimator/MagBiasEstimator.cpp index 4ef331a0710a..e386170a210c 100644 --- a/src/modules/mag_bias_estimator/MagBiasEstimator.cpp +++ b/src/modules/mag_bias_estimator/MagBiasEstimator.cpp @@ -157,10 +157,11 @@ void MagBiasEstimator::Run() bool updated = false; for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) { + int sensor_mag_updates = 0; sensor_mag_s sensor_mag; - while (_sensor_mag_subs[mag_index].update(&sensor_mag)) { - + while ((sensor_mag_updates < sensor_mag_s::ORB_QUEUE_LENGTH) && _sensor_mag_subs[mag_index].update(&sensor_mag)) { + sensor_mag_updates++; updated = true; // apply existing mag calibration diff --git a/src/modules/mavlink/mavlink b/src/modules/mavlink/mavlink index 5f85bd7d7d61..a3558d6b335d 160000 --- a/src/modules/mavlink/mavlink +++ b/src/modules/mavlink/mavlink @@ -1 +1 @@ -Subproject commit 5f85bd7d7d6155d2d349bd04ed67544610e8e65b +Subproject commit a3558d6b335d930fc01816fd168d16b3f38ed434 diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index c730dd0f4775..2467554e97a5 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -124,13 +124,13 @@ Mavlink::Mavlink() : // ensure topic exists, otherwise we might lose first queued commands if (orb_exists(ORB_ID(vehicle_command), 0) == PX4_ERROR) { - orb_advertise_queue(ORB_ID(vehicle_command), nullptr, vehicle_command_s::ORB_QUEUE_LENGTH); + orb_advertise(ORB_ID(vehicle_command), nullptr); } _vehicle_command_sub.subscribe(); if (orb_exists(ORB_ID(event), 0) == PX4_ERROR) { - orb_advertise_queue(ORB_ID(event), nullptr, event_s::ORB_QUEUE_LENGTH); + orb_advertise(ORB_ID(event), nullptr); } _event_sub.subscribe(); @@ -1669,6 +1669,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("SCALED_IMU2", 25.0f); configure_stream_local("SCALED_IMU3", 25.0f); configure_stream_local("SCALED_PRESSURE", 1.0f); + configure_stream_local("SCALED_PRESSURE2", 1.0f); configure_stream_local("SERVO_OUTPUT_RAW_0", 20.0f); configure_stream_local("SERVO_OUTPUT_RAW_1", 20.0f); configure_stream_local("SYS_STATUS", 1.0f); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index d9a9dc213910..bcd532861814 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -132,6 +132,7 @@ #if !defined(CONSTRAINED_FLASH) # include "streams/ADSB_VEHICLE.hpp" # include "streams/AUTOPILOT_STATE_FOR_GIMBAL_DEVICE.hpp" +# include "streams/BATTERY_INFO.hpp" # include "streams/DEBUG.hpp" # include "streams/DEBUG_FLOAT_ARRAY.hpp" # include "streams/DEBUG_VECT.hpp" @@ -147,7 +148,6 @@ # include "streams/ODOMETRY.hpp" # include "streams/SCALED_PRESSURE2.hpp" # include "streams/SCALED_PRESSURE3.hpp" -# include "streams/SMART_BATTERY_INFO.hpp" # include "streams/UAVIONIX_ADSB_OUT_CFG.hpp" # include "streams/UAVIONIX_ADSB_OUT_DYNAMIC.hpp" # include "streams/UTM_GLOBAL_POSITION.hpp" @@ -259,9 +259,9 @@ static const StreamListItem streams_list[] = { create_stream_list_item(), #endif // SYS_STATUS_HPP create_stream_list_item(), -#if defined(SMART_BATTERY_INFO_HPP) - create_stream_list_item(), -#endif // SMART_BATTERY_INFO_HPP +#if defined(BATTERY_INFO_HPP) + create_stream_list_item(), +#endif // BATTERY_INFO_HPP #if defined(HIGHRES_IMU_HPP) create_stream_list_item(), #endif // HIGHRES_IMU_HPP diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index bc0ed3d0734c..5057c1625dbe 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -57,7 +57,9 @@ using matrix::wrap_2pi; -dm_item_t MavlinkMissionManager::_dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0; +dm_item_t MavlinkMissionManager::_mission_dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0; +dm_item_t MavlinkMissionManager::_safepoint_dataman_id = DM_KEY_SAFE_POINTS_0; +dm_item_t MavlinkMissionManager::_fence_dataman_id = DM_KEY_FENCE_POINTS_0; bool MavlinkMissionManager::_dataman_init = false; uint16_t MavlinkMissionManager::_count[3] = { 0, 0, 0 }; uint32_t MavlinkMissionManager::_crc32[3] = { 0, 0, 0 }; @@ -88,15 +90,21 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : } else { PX4_WARN("offboard mission init failed"); } + + update_active_mission(_mission_dataman_id, _count[MAV_MISSION_TYPE_MISSION], _current_seq, + _crc32[MAV_MISSION_TYPE_MISSION], false); } - _my_dataman_id = _dataman_id; + _my_mission_dataman_id = _mission_dataman_id; + _my_fence_dataman_id = _fence_dataman_id; + _my_safepoint_dataman_id = _safepoint_dataman_id; } void MavlinkMissionManager::init_offboard_mission(const mission_s &mission_state) { - _dataman_id = (dm_item_t)mission_state.dataman_id; + _mission_dataman_id = (dm_item_t)mission_state.mission_dataman_id; + _my_mission_dataman_id = _mission_dataman_id; _count[MAV_MISSION_TYPE_MISSION] = mission_state.count; _crc32[MAV_MISSION_TYPE_MISSION] = mission_state.mission_id; _current_seq = mission_state.current_seq; @@ -109,12 +117,14 @@ MavlinkMissionManager::load_geofence_stats() { mission_stats_entry_s stats; // initialize fence points count - bool success = _dataman_client.readSync(DM_KEY_FENCE_POINTS, 0, reinterpret_cast(&stats), + bool success = _dataman_client.readSync(DM_KEY_FENCE_POINTS_STATE, 0, reinterpret_cast(&stats), sizeof(mission_stats_entry_s)); if (success) { _count[MAV_MISSION_TYPE_FENCE] = stats.num_items; _crc32[MAV_MISSION_TYPE_FENCE] = stats.opaque_id; + _fence_dataman_id = static_cast(stats.dataman_id); + _my_fence_dataman_id = _fence_dataman_id; } return success; @@ -125,12 +135,14 @@ MavlinkMissionManager::load_safepoint_stats() { mission_stats_entry_s stats; // initialize safe points count - bool success = _dataman_client.readSync(DM_KEY_SAFE_POINTS, 0, reinterpret_cast(&stats), + bool success = _dataman_client.readSync(DM_KEY_SAFE_POINTS_STATE, 0, reinterpret_cast(&stats), sizeof(mission_stats_entry_s)); if (success) { _count[MAV_MISSION_TYPE_RALLY] = stats.num_items; _crc32[MAV_MISSION_TYPE_RALLY] = stats.opaque_id; + _safepoint_dataman_id = static_cast(stats.dataman_id); + _my_safepoint_dataman_id = _safepoint_dataman_id; } return success; @@ -140,19 +152,21 @@ MavlinkMissionManager::load_safepoint_stats() * Publish mission topic to notify navigator about changes. */ void -MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq, uint32_t crc32, +MavlinkMissionManager::update_active_mission(dm_item_t mission_dataman_id, uint16_t count, int32_t seq, uint32_t crc32, bool write_to_dataman) { /* update active mission state */ - _dataman_id = dataman_id; + _mission_dataman_id = mission_dataman_id; + _my_mission_dataman_id = _mission_dataman_id; _count[MAV_MISSION_TYPE_MISSION] = count; _crc32[MAV_MISSION_TYPE_MISSION] = crc32; _current_seq = seq; - _my_dataman_id = _dataman_id; mission_s mission{}; mission.timestamp = hrt_absolute_time(); - mission.dataman_id = dataman_id; + mission.mission_dataman_id = mission_dataman_id; + mission.fence_dataman_id = _fence_dataman_id; + mission.safepoint_dataman_id = _safepoint_dataman_id; mission.count = count; mission.current_seq = seq; mission.mission_id = _crc32[MAV_MISSION_TYPE_MISSION]; @@ -174,14 +188,18 @@ MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t coun } int -MavlinkMissionManager::update_geofence_count(unsigned count, uint32_t crc32) +MavlinkMissionManager::update_geofence_count(dm_item_t fence_dataman_id, unsigned count, uint32_t crc32) { + _fence_dataman_id = fence_dataman_id; + _my_fence_dataman_id = fence_dataman_id; + mission_stats_entry_s stats; stats.num_items = count; stats.opaque_id = crc32; + stats.dataman_id = fence_dataman_id; /* update stats in dataman */ - bool success = _dataman_client.writeSync(DM_KEY_FENCE_POINTS, 0, reinterpret_cast(&stats), + bool success = _dataman_client.writeSync(DM_KEY_FENCE_POINTS_STATE, 0, reinterpret_cast(&stats), sizeof(mission_stats_entry_s)); if (success) { @@ -199,20 +217,25 @@ MavlinkMissionManager::update_geofence_count(unsigned count, uint32_t crc32) return PX4_ERROR; } - update_active_mission(_dataman_id, _count[MAV_MISSION_TYPE_MISSION], _current_seq, _crc32[MAV_MISSION_TYPE_MISSION], + update_active_mission(_mission_dataman_id, _count[MAV_MISSION_TYPE_MISSION], _current_seq, + _crc32[MAV_MISSION_TYPE_MISSION], false); return PX4_OK; } int -MavlinkMissionManager::update_safepoint_count(unsigned count, uint32_t crc32) +MavlinkMissionManager::update_safepoint_count(dm_item_t safepoint_dataman_id, unsigned count, uint32_t crc32) { + _safepoint_dataman_id = safepoint_dataman_id; + _my_safepoint_dataman_id = safepoint_dataman_id; + mission_stats_entry_s stats; stats.num_items = count; stats.opaque_id = crc32; + stats.dataman_id = safepoint_dataman_id; /* update stats in dataman */ - bool success = _dataman_client.writeSync(DM_KEY_SAFE_POINTS, 0, reinterpret_cast(&stats), + bool success = _dataman_client.writeSync(DM_KEY_SAFE_POINTS_STATE, 0, reinterpret_cast(&stats), sizeof(mission_stats_entry_s)); if (success) { @@ -230,7 +253,8 @@ MavlinkMissionManager::update_safepoint_count(unsigned count, uint32_t crc32) return PX4_ERROR; } - update_active_mission(_dataman_id, _count[MAV_MISSION_TYPE_MISSION], _current_seq, _crc32[MAV_MISSION_TYPE_MISSION], + update_active_mission(_mission_dataman_id, _count[MAV_MISSION_TYPE_MISSION], _current_seq, + _crc32[MAV_MISSION_TYPE_MISSION], false); return PX4_OK; } @@ -293,14 +317,14 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t switch (_mission_type) { case MAV_MISSION_TYPE_MISSION: { - read_success = _dataman_client.readSync(_dataman_id, seq, reinterpret_cast(&mission_item), + read_success = _dataman_client.readSync(_mission_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; - read_success = _dataman_client.readSync(DM_KEY_FENCE_POINTS, seq + 1, + read_success = _dataman_client.readSync(_fence_dataman_id, seq, reinterpret_cast(&mission_fence_point), sizeof(mission_fence_point_s)); mission_item.nav_cmd = mission_fence_point.nav_cmd; @@ -320,7 +344,7 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t break; case MAV_MISSION_TYPE_RALLY: { // Read a safe point / rally point - read_success = _dataman_client.readSync(DM_KEY_SAFE_POINTS, seq + 1, reinterpret_cast(&mission_item), + read_success = _dataman_client.readSync(_safepoint_dataman_id, seq, reinterpret_cast(&mission_item), sizeof(mission_item_s)); } break; @@ -375,7 +399,7 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t "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); + PX4_DEBUG("WPM: Send MISSION_ITEM ERROR: could not read seq %u from dataman ID %i", seq, _mission_dataman_id); } } @@ -670,7 +694,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]) { - update_active_mission(_dataman_id, _count[MAV_MISSION_TYPE_MISSION], wpc.seq, _crc32[MAV_MISSION_TYPE_MISSION]); + update_active_mission(_mission_dataman_id, _count[MAV_MISSION_TYPE_MISSION], wpc.seq, _crc32[MAV_MISSION_TYPE_MISSION]); } else { PX4_ERR("WPM: MISSION_SET_CURRENT seq=%d ERROR: not in list", wpc.seq); @@ -904,7 +928,7 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) /* alternate dataman ID anyway to let navigator know about changes */ - if (_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0) { + if (_mission_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0) { update_active_mission(DM_KEY_WAYPOINTS_OFFBOARD_1, 0, 0, 0); } else { @@ -914,11 +938,12 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) break; case MAV_MISSION_TYPE_FENCE: - update_geofence_count(0, 0); + update_geofence_count(_fence_dataman_id == DM_KEY_FENCE_POINTS_0 ? DM_KEY_FENCE_POINTS_1 : DM_KEY_FENCE_POINTS_0, 0, 0); break; case MAV_MISSION_TYPE_RALLY: - update_safepoint_count(0, 0); + update_safepoint_count(_safepoint_dataman_id == DM_KEY_SAFE_POINTS_0 ? DM_KEY_SAFE_POINTS_1 : DM_KEY_SAFE_POINTS_0, 0, + 0); break; default: @@ -933,13 +958,34 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) PX4_DEBUG("WPM: MISSION_COUNT %u from ID %u, changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); + switch (_mission_type) { + case MAV_MISSION_TYPE_MISSION: + _transfer_dataman_id = (_mission_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : + DM_KEY_WAYPOINTS_OFFBOARD_0); // use inactive storage for transmission + break; + + case MAV_MISSION_TYPE_FENCE: + _transfer_dataman_id = (_fence_dataman_id == DM_KEY_FENCE_POINTS_0 ? DM_KEY_FENCE_POINTS_1 : + DM_KEY_FENCE_POINTS_0); // use inactive storage for transmission + break; + + case MAV_MISSION_TYPE_RALLY: + _transfer_dataman_id = (_safepoint_dataman_id == DM_KEY_SAFE_POINTS_0 ? DM_KEY_SAFE_POINTS_1 : + DM_KEY_SAFE_POINTS_0); // use inactive storage for transmission + break; + + default: + PX4_ERR("mission type %u not handled", _mission_type); + _transfer_in_progress = false; + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_INVALID); + return; + } + _state = MAVLINK_WPM_STATE_GETLIST; _transfer_seq = 0; _transfer_partner_sysid = msg->sysid; _transfer_partner_compid = msg->compid; _transfer_count = wpc.count; - _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; _transfer_land_start_marker = -1; _transfer_land_marker = -1; @@ -1096,7 +1142,8 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) } else { - write_failed = !_dataman_client.writeSync(_transfer_dataman_id, wp.seq, reinterpret_cast(&mission_item), + write_failed = !_dataman_client.writeSync(_transfer_dataman_id, wp.seq, + reinterpret_cast(&mission_item), sizeof(struct mission_item_s)); // Check for land start marker @@ -1138,7 +1185,6 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) if (mission_item.vertex_count < 3) { // feasibility check PX4_ERR("Fence: too few vertices"); check_failed = true; - update_geofence_count(0, 0); } } else { @@ -1148,7 +1194,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) mission_fence_point.frame = mission_item.frame; if (!check_failed) { - write_failed = !_dataman_client.writeSync(DM_KEY_FENCE_POINTS, wp.seq + 1, + write_failed = !_dataman_client.writeSync(_transfer_dataman_id, wp.seq, reinterpret_cast(&mission_fence_point), sizeof(mission_fence_point_s)); } @@ -1156,7 +1202,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) break; case MAV_MISSION_TYPE_RALLY: { // Write a safe point / rally point - write_failed = !_dataman_client.writeSync(DM_KEY_SAFE_POINTS, wp.seq + 1, + write_failed = !_dataman_client.writeSync(_transfer_dataman_id, wp.seq, reinterpret_cast(&mission_item), sizeof(mission_item_s), 2_s); } break; @@ -1204,15 +1250,30 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) case MAV_MISSION_TYPE_MISSION: _land_start_marker = _transfer_land_start_marker; _land_marker = _transfer_land_marker; - update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq, _transfer_current_crc32); + + // Only need to update if the mission actually changed + if (_transfer_current_crc32 != _crc32[MAV_MISSION_TYPE_MISSION]) { + update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq, _transfer_current_crc32); + } + break; case MAV_MISSION_TYPE_FENCE: - ret = update_geofence_count(_transfer_count, _transfer_current_crc32); + + // Only need to update if the mission actually changed + if (_transfer_current_crc32 != _crc32[MAV_MISSION_TYPE_FENCE]) { + ret = update_geofence_count(_transfer_dataman_id, _transfer_count, _transfer_current_crc32); + } + break; case MAV_MISSION_TYPE_RALLY: - ret = update_safepoint_count(_transfer_count, _transfer_current_crc32); + + // Only need to update if the mission actually changed + if (_transfer_current_crc32 != _crc32[MAV_MISSION_TYPE_RALLY]) { + ret = update_safepoint_count(_transfer_dataman_id, _transfer_count, _transfer_current_crc32); + } + break; default: @@ -1260,25 +1321,29 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg) case MAV_MISSION_TYPE_MISSION: _land_start_marker = -1; _land_marker = -1; - update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : + update_active_mission(_mission_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0, 0); break; case MAV_MISSION_TYPE_FENCE: - ret = update_geofence_count(0, 0); + ret = update_geofence_count(_fence_dataman_id == DM_KEY_FENCE_POINTS_0 ? DM_KEY_FENCE_POINTS_1 : DM_KEY_FENCE_POINTS_0, + 0, 0); break; case MAV_MISSION_TYPE_RALLY: - ret = update_safepoint_count(0, 0); + ret = update_safepoint_count(_safepoint_dataman_id == DM_KEY_SAFE_POINTS_0 ? DM_KEY_SAFE_POINTS_1 : + DM_KEY_SAFE_POINTS_0, 0, 0); break; case MAV_MISSION_TYPE_ALL: _land_start_marker = -1; _land_marker = -1; - update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : + update_active_mission(_mission_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0, 0); - ret = update_geofence_count(0, 0); - ret = update_safepoint_count(0, 0) || ret; + ret = update_geofence_count(_fence_dataman_id == DM_KEY_FENCE_POINTS_0 ? DM_KEY_FENCE_POINTS_1 : DM_KEY_FENCE_POINTS_0, + 0, 0); + ret = update_safepoint_count(_safepoint_dataman_id == DM_KEY_SAFE_POINTS_0 ? DM_KEY_SAFE_POINTS_1 : + DM_KEY_SAFE_POINTS_0, 0, 0) || ret; break; default: @@ -1781,19 +1846,20 @@ void MavlinkMissionManager::check_active_mission() if (_mission_sub.updated()) { _mission_sub.update(); - if (_mission_sub.get().geofence_id != _crc32[MAV_MISSION_TYPE_FENCE]) { + if ((_mission_sub.get().geofence_id != _crc32[MAV_MISSION_TYPE_FENCE]) + || (_my_fence_dataman_id != (dm_item_t) _mission_sub.get().fence_dataman_id)) { load_geofence_stats(); } - if (_mission_sub.get().safe_points_id != _crc32[MAV_MISSION_TYPE_RALLY]) { + if ((_mission_sub.get().safe_points_id != _crc32[MAV_MISSION_TYPE_RALLY]) + || (_my_safepoint_dataman_id != (dm_item_t) _mission_sub.get().safepoint_dataman_id)) { load_safepoint_stats(); } if ((_mission_sub.get().mission_id != _crc32[MAV_MISSION_TYPE_MISSION]) - || (_my_dataman_id != (dm_item_t)_mission_sub.get().dataman_id)) { + || (_my_mission_dataman_id != (dm_item_t)_mission_sub.get().mission_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, _crc32[MAV_MISSION_TYPE_MISSION]); } diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index 971787a54734..2251ef9e61da 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -106,8 +106,12 @@ class MavlinkMissionManager unsigned _filesystem_errcount{0}; ///< File system error count - static dm_item_t _dataman_id; ///< Global Dataman storage ID for active mission - dm_item_t _my_dataman_id{DM_KEY_WAYPOINTS_OFFBOARD_0}; ///< class Dataman storage ID + static dm_item_t _mission_dataman_id; ///< Global Dataman storage ID for active mission + static dm_item_t _safepoint_dataman_id; ///< Global dataman storage id for active safepoints + static dm_item_t _fence_dataman_id; ///< Global dataman storage id for active geofence + dm_item_t _my_mission_dataman_id{DM_KEY_WAYPOINTS_OFFBOARD_0}; ///< class Dataman storage ID for mission + dm_item_t _my_safepoint_dataman_id{DM_KEY_SAFE_POINTS_0}; ///< class Dataman storage ID for safepoints + dm_item_t _my_fence_dataman_id{DM_KEY_FENCE_POINTS_0}; ///< class Dataman storage ID for geofence static bool _dataman_init; ///< Dataman initialized @@ -117,7 +121,7 @@ class MavlinkMissionManager int32_t _last_reached{-1}; ///< Last reached waypoint in active mission (-1 means nothing reached) - dm_item_t _transfer_dataman_id{DM_KEY_WAYPOINTS_OFFBOARD_1}; ///< Dataman storage ID for current transmission + dm_item_t _transfer_dataman_id{DM_KEY_WAYPOINTS_OFFBOARD_1}; ///< Dataman storage ID for current transmission uint16_t _transfer_count{0}; ///< Items count in current transmission uint32_t _transfer_current_crc32{0}; ///< Current CRC32 checksum of current transmission @@ -148,8 +152,8 @@ class MavlinkMissionManager 2; ///< Error count limit before stopping to report FS errors static constexpr uint16_t MAX_COUNT[] = { DM_KEY_WAYPOINTS_OFFBOARD_0_MAX, - DM_KEY_FENCE_POINTS_MAX - 1, - DM_KEY_SAFE_POINTS_MAX - 1 + DM_KEY_FENCE_POINTS_MAX, + DM_KEY_SAFE_POINTS_MAX }; /**< Maximum number of mission items for each type (fence & safe points use the first item for the stats) */ @@ -168,14 +172,14 @@ class MavlinkMissionManager void init_offboard_mission(const mission_s &mission_state); - void update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq, uint32_t crc32, + void update_active_mission(dm_item_t mission_dataman_id, uint16_t count, int32_t seq, uint32_t crc32, bool write_to_dataman = true); /** store the geofence count to dataman */ - int update_geofence_count(unsigned count, uint32_t crc32); + int update_geofence_count(dm_item_t fence_dataman_id, unsigned count, uint32_t crc32); /** store the safepoint count to dataman */ - int update_safepoint_count(unsigned count, uint32_t crc32); + int update_safepoint_count(dm_item_t safepoint_dataman_id, unsigned count, uint32_t crc32); /** load geofence stats from dataman */ bool load_geofence_stats(); diff --git a/src/modules/mavlink/streams/SMART_BATTERY_INFO.hpp b/src/modules/mavlink/streams/BATTERY_INFO.hpp similarity index 67% rename from src/modules/mavlink/streams/SMART_BATTERY_INFO.hpp rename to src/modules/mavlink/streams/BATTERY_INFO.hpp index 3476db3a02b2..fd72acce45ed 100644 --- a/src/modules/mavlink/streams/SMART_BATTERY_INFO.hpp +++ b/src/modules/mavlink/streams/BATTERY_INFO.hpp @@ -31,30 +31,30 @@ * ****************************************************************************/ -#ifndef SMART_BATTERY_INFO_HPP -#define SMART_BATTERY_INFO_HPP +#ifndef BATTERY_INFO_HPP +#define BATTERY_INFO_HPP #include -class MavlinkStreamSmartBatteryInfo : public MavlinkStream +class MavlinkStreamBatteryInfo : public MavlinkStream { public: - static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamSmartBatteryInfo(mavlink); } + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamBatteryInfo(mavlink); } - static constexpr const char *get_name_static() { return "SMART_BATTERY_INFO"; } - static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_SMART_BATTERY_INFO; } + static constexpr const char *get_name_static() { return "BATTERY_INFO"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_BATTERY_INFO; } const char *get_name() const override { return get_name_static(); } uint16_t get_id() override { return get_id_static(); } unsigned get_size() override { - static constexpr unsigned size_per_battery = MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + static constexpr unsigned size_per_battery = MAVLINK_MSG_ID_BATTERY_INFO_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; return size_per_battery * _battery_status_subs.advertised_count(); } private: - explicit MavlinkStreamSmartBatteryInfo(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamBatteryInfo(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::SubscriptionMultiArray _battery_status_subs{ORB_ID::battery_status}; @@ -67,36 +67,50 @@ class MavlinkStreamSmartBatteryInfo : public MavlinkStream if (battery_sub.update(&battery_status)) { if (battery_status.serial_number == 0) { - // This is not smart battery + // Required to emit continue; } - mavlink_smart_battery_info_t msg{}; + mavlink_battery_info_t msg{}; msg.id = battery_status.id - 1; - msg.capacity_full_specification = battery_status.capacity; - msg.capacity_full = (int32_t)((float)(battery_status.state_of_health * battery_status.capacity) / 100.f); + msg.design_capacity = (float)(battery_status.capacity * 1000); + msg.full_charge_capacity = (float)(battery_status.state_of_health * battery_status.capacity * 1000.f) / 100.f; msg.cycle_count = battery_status.cycle_count; if (battery_status.manufacture_date) { uint16_t day = battery_status.manufacture_date % 32; uint16_t month = (battery_status.manufacture_date >> 5) % 16; - uint16_t year = (80 + (battery_status.manufacture_date >> 9)) % 100; + uint16_t year = (80 + (battery_status.manufacture_date >> 9)); + uint16_t year2dig = year % 100; + //Formatted as 'ddmmyyyy' (maxed 9 chars) + snprintf(msg.manufacture_date, sizeof(msg.manufacture_date), "%d%d%d", day, month, year); //Formatted as 'dd/mm/yy-123456' (maxed 15 + 1 chars) - snprintf(msg.serial_number, sizeof(msg.serial_number), "%d/%d/%d-%d", day, month, year, battery_status.serial_number); + snprintf(msg.serial_number, sizeof(msg.serial_number), "%d/%d/%d-%d", day, month, year2dig, + battery_status.serial_number); } else { + snprintf(msg.serial_number, sizeof(msg.serial_number), "%d", battery_status.serial_number); } - //msg.device_name = ?? - msg.weight = -1; - msg.discharge_minimum_voltage = -1; - msg.charging_minimum_voltage = -1; - msg.resting_minimum_voltage = -1; - - mavlink_msg_smart_battery_info_send_struct(_mavlink->get_channel(), &msg); + // Not supported by PX4 (not in battery_status uorb topic) + /* + msg.name = 0; // char[50] + msg.weight = 0; + msg.discharge_minimum_voltage = 0; + msg.charging_minimum_voltage = 0; + msg.resting_minimum_voltage = 0; + msg.charging_maximum_voltage = 0; + msg.charging_maximum_current = 0; + msg.discharge_maximum_current = 0; + msg.discharge_maximum_burst_current = 0; + msg.cells_in_series = 0; + msg.nominal_voltage = 0; + */ + + mavlink_msg_battery_info_send_struct(_mavlink->get_channel(), &msg); updated = true; } } @@ -105,4 +119,4 @@ class MavlinkStreamSmartBatteryInfo : public MavlinkStream } }; -#endif // SMART_BATTERY_INFO_HPP +#endif // BATTERY_INFO_HPP diff --git a/src/modules/mavlink/streams/OPEN_DRONE_ID_SYSTEM.hpp b/src/modules/mavlink/streams/OPEN_DRONE_ID_SYSTEM.hpp index bcc7e0f1452e..9467df81e955 100644 --- a/src/modules/mavlink/streams/OPEN_DRONE_ID_SYSTEM.hpp +++ b/src/modules/mavlink/streams/OPEN_DRONE_ID_SYSTEM.hpp @@ -86,7 +86,8 @@ class MavlinkStreamOpenDroneIdSystem : public MavlinkStream msg.area_floor = -1000; msg.category_eu = MAV_ODID_CATEGORY_EU_UNDECLARED; msg.class_eu = MAV_ODID_CLASS_EU_UNDECLARED; - msg.operator_altitude_geo = home_position.alt; + float wgs84_amsl_offset = vehicle_gps_position.altitude_ellipsoid_m - vehicle_gps_position.altitude_msl_m; + msg.operator_altitude_geo = home_position.alt + wgs84_amsl_offset; // timestamp: 32 bit Unix Timestamp in seconds since 00:00:00 01/01/2019. static uint64_t utc_offset_s = 1'546'300'800; // UTC seconds since 00:00:00 01/01/2019 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 35c65d59110e..39cb033e74dc 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -192,11 +192,6 @@ MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt, attitude_setpoint.timestamp = hrt_absolute_time(); _vehicle_attitude_setpoint_pub.publish(attitude_setpoint); - - // update attitude controller setpoint immediately - _attitude_control.setAttitudeSetpoint(q_sp, attitude_setpoint.yaw_sp_move_rate); - _thrust_setpoint_body = Vector3f(attitude_setpoint.thrust_body); - _last_attitude_setpoint = attitude_setpoint.timestamp; } void @@ -231,34 +226,6 @@ MulticopterAttitudeControl::Run() const Quatf q{v_att.q}; - // Check for new attitude setpoint - if (_vehicle_attitude_setpoint_sub.updated()) { - vehicle_attitude_setpoint_s vehicle_attitude_setpoint; - - if (_vehicle_attitude_setpoint_sub.copy(&vehicle_attitude_setpoint) - && (vehicle_attitude_setpoint.timestamp > _last_attitude_setpoint)) { - - _attitude_control.setAttitudeSetpoint(Quatf(vehicle_attitude_setpoint.q_d), vehicle_attitude_setpoint.yaw_sp_move_rate); - _thrust_setpoint_body = Vector3f(vehicle_attitude_setpoint.thrust_body); - _last_attitude_setpoint = vehicle_attitude_setpoint.timestamp; - } - } - - // Check for a heading reset - if (_quat_reset_counter != v_att.quat_reset_counter) { - const Quatf delta_q_reset(v_att.delta_q_reset); - - // for stabilized attitude generation only extract the heading change from the delta quaternion - _man_yaw_sp = wrap_pi(_man_yaw_sp + Eulerf(delta_q_reset).psi()); - - if (v_att.timestamp > _last_attitude_setpoint) { - // adapt existing attitude setpoint unless it was generated after the current attitude estimate - _attitude_control.adaptAttitudeSetpoint(delta_q_reset); - } - - _quat_reset_counter = v_att.quat_reset_counter; - } - /* check for updates in other topics */ _manual_control_setpoint_sub.update(&_manual_control_setpoint); _vehicle_control_mode_sub.update(&_vehicle_control_mode); @@ -300,7 +267,8 @@ MulticopterAttitudeControl::Run() // vehicle is a tailsitter in transition mode const bool is_tailsitter_transition = (_vtol_tailsitter && _vtol_in_transition_mode); - bool run_att_ctrl = _vehicle_control_mode.flag_control_attitude_enabled && (is_hovering || is_tailsitter_transition); + const bool run_att_ctrl = _vehicle_control_mode.flag_control_attitude_enabled && (is_hovering + || is_tailsitter_transition); if (run_att_ctrl) { @@ -318,6 +286,34 @@ MulticopterAttitudeControl::Run() _man_pitch_input_filter.reset(0.f); } + // Check for new attitude setpoint + if (_vehicle_attitude_setpoint_sub.updated()) { + vehicle_attitude_setpoint_s vehicle_attitude_setpoint; + + if (_vehicle_attitude_setpoint_sub.copy(&vehicle_attitude_setpoint) + && (vehicle_attitude_setpoint.timestamp > _last_attitude_setpoint)) { + + _attitude_control.setAttitudeSetpoint(Quatf(vehicle_attitude_setpoint.q_d), vehicle_attitude_setpoint.yaw_sp_move_rate); + _thrust_setpoint_body = Vector3f(vehicle_attitude_setpoint.thrust_body); + _last_attitude_setpoint = vehicle_attitude_setpoint.timestamp; + } + } + + // Check for a heading reset + if (_quat_reset_counter != v_att.quat_reset_counter) { + const Quatf delta_q_reset(v_att.delta_q_reset); + + // for stabilized attitude generation only extract the heading change from the delta quaternion + _man_yaw_sp = wrap_pi(_man_yaw_sp + Eulerf(delta_q_reset).psi()); + + if (v_att.timestamp > _last_attitude_setpoint) { + // adapt existing attitude setpoint unless it was generated after the current attitude estimate + _attitude_control.adaptAttitudeSetpoint(delta_q_reset); + } + + _quat_reset_counter = v_att.quat_reset_counter; + } + Vector3f rates_sp = _attitude_control.update(q); const hrt_abstime now = hrt_absolute_time(); diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index ca8360c80452..90928921835d 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -167,6 +167,7 @@ void MulticopterPositionControl::parameters_update(bool force) Vector3f(_param_mpc_xy_vel_i_acc.get(), _param_mpc_xy_vel_i_acc.get(), _param_mpc_z_vel_i_acc.get()), Vector3f(_param_mpc_xy_vel_d_acc.get(), _param_mpc_xy_vel_d_acc.get(), _param_mpc_z_vel_d_acc.get())); _control.setHorizontalThrustMargin(_param_mpc_thr_xy_marg.get()); + _control.decoupleHorizontalAndVecticalAcceleration(_param_mpc_acc_decouple.get()); _goto_control.setParamMpcAccHor(_param_mpc_acc_hor.get()); _goto_control.setParamMpcAccDownMax(_param_mpc_acc_down_max.get()); _goto_control.setParamMpcAccUpMax(_param_mpc_acc_up_max.get()); diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp index b1f88fde57d3..3dac59bc1b89 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -146,6 +146,7 @@ class MulticopterPositionControl : public ModuleBase (ParamFloat) _param_mpc_tiltmax_air, (ParamFloat) _param_mpc_thr_hover, (ParamBool) _param_mpc_use_hte, + (ParamBool) _param_mpc_acc_decouple, // Takeoff / Land (ParamFloat) _param_com_spoolup_time, /**< time to let motors spool up after arming */ diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index 955f3f4b44a7..0cf632c04359 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -204,13 +204,20 @@ void PositionControl::_velocityControl(const float dt) void PositionControl::_accelerationControl() { // Assume standard acceleration due to gravity in vertical direction for attitude generation - Vector3f body_z = Vector3f(-_acc_sp(0), -_acc_sp(1), CONSTANTS_ONE_G).normalized(); + float z_specific_force = -CONSTANTS_ONE_G; + + if (!_decouple_horizontal_and_vertical_acceleration) { + // Include vertical acceleration setpoint for better horizontal acceleration tracking + z_specific_force += _acc_sp(2); + } + + Vector3f body_z = Vector3f(-_acc_sp(0), -_acc_sp(1), -z_specific_force).normalized(); ControlMath::limitTilt(body_z, Vector3f(0, 0, 1), _lim_tilt); - // Scale thrust assuming hover thrust produces standard gravity - float collective_thrust = _acc_sp(2) * (_hover_thrust / CONSTANTS_ONE_G) - _hover_thrust; + // Convert to thrust assuming hover thrust produces standard gravity + const float thrust_ned_z = _acc_sp(2) * (_hover_thrust / CONSTANTS_ONE_G) - _hover_thrust; // Project thrust to planned body attitude - collective_thrust /= (Vector3f(0, 0, 1).dot(body_z)); - collective_thrust = math::min(collective_thrust, -_lim_thr_min); + const float cos_ned_body = (Vector3f(0, 0, 1).dot(body_z)); + const float collective_thrust = math::min(thrust_ned_z / cos_ned_body, -_lim_thr_min); _thr_sp = body_z * collective_thrust; } diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp index 4eb909e1fa3e..85af876cd136 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp @@ -163,6 +163,11 @@ class PositionControl */ void resetIntegral() { _vel_int.setZero(); } + /** + * If set, the tilt setpoint is computed by assuming no vertical acceleration + */ + void decoupleHorizontalAndVecticalAcceleration(bool val) { _decouple_horizontal_and_vertical_acceleration = val; } + /** * Get the controllers output local position setpoint * These setpoints are the ones which were executed on including PID output and feed-forward. @@ -211,6 +216,7 @@ class PositionControl float _lim_tilt{}; ///< Maximum tilt from level the output attitude is allowed to have float _hover_thrust{}; ///< Thrust [HOVER_THRUST_MIN, HOVER_THRUST_MAX] with which the vehicle hovers not accelerating down or up with level orientation + bool _decouple_horizontal_and_vertical_acceleration{true}; ///< Ignore vertical acceleration setpoint to remove its effect on the tilt setpoint // States matrix::Vector3f _pos; /**< current position */ diff --git a/src/modules/mc_pos_control/multicopter_position_control_limits_params.c b/src/modules/mc_pos_control/multicopter_position_control_limits_params.c index a60732cfa8c2..91039f04ea6d 100644 --- a/src/modules/mc_pos_control/multicopter_position_control_limits_params.c +++ b/src/modules/mc_pos_control/multicopter_position_control_limits_params.c @@ -138,3 +138,13 @@ PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f); * @group Multicopter Position Control */ PARAM_DEFINE_FLOAT(MPC_THR_MAX, 1.f); + +/** + * Acceleration to tilt coupling + * + * Set to decouple tilt from vertical acceleration. + * + * @boolean + * @group Multicopter Position Control + */ +PARAM_DEFINE_INT32(MPC_ACC_DECOUPLE, 1); diff --git a/src/modules/mc_pos_control/multicopter_stabilized_mode_params.c b/src/modules/mc_pos_control/multicopter_stabilized_mode_params.c index f478f9938e27..481d9cce66f9 100644 --- a/src/modules/mc_pos_control/multicopter_stabilized_mode_params.c +++ b/src/modules/mc_pos_control/multicopter_stabilized_mode_params.c @@ -35,8 +35,8 @@ * Maximal tilt angle in Stabilized or Altitude mode * * @unit deg - * @min 0 - * @max 90 + * @min 1 + * @max 70 * @decimal 0 * @increment 1 * @group Multicopter Position Control diff --git a/src/modules/muorb/apps/CMakeLists.txt b/src/modules/muorb/apps/CMakeLists.txt index 55573cde3573..532ada62413b 100644 --- a/src/modules/muorb/apps/CMakeLists.txt +++ b/src/modules/muorb/apps/CMakeLists.txt @@ -39,6 +39,7 @@ px4_add_module( INCLUDES ../test ../aggregator + ${PX4_BOARD_DIR}/libfc-sensor-api/inc SRCS uORBAppsProtobufChannel.cpp muorb_main.cpp diff --git a/src/modules/muorb/apps/Kconfig b/src/modules/muorb/apps/Kconfig index 21969d591d13..f0afac8b6204 100644 --- a/src/modules/muorb/apps/Kconfig +++ b/src/modules/muorb/apps/Kconfig @@ -4,3 +4,11 @@ menuconfig MODULES_MUORB_APPS depends on PLATFORM_POSIX ---help--- Enable support for muorb apps + + + config MUORB_APPS_SYNC_TIMESTAMP + bool "Sync timestamp with external processor" + depends on MODULES_MUORB_APPS + default y + help + causes HRT timestamp to use an externally calculated offset for synchronization diff --git a/src/modules/muorb/apps/uORBAppsProtobufChannel.cpp b/src/modules/muorb/apps/uORBAppsProtobufChannel.cpp index 963416547981..8669d0650d6a 100644 --- a/src/modules/muorb/apps/uORBAppsProtobufChannel.cpp +++ b/src/modules/muorb/apps/uORBAppsProtobufChannel.cpp @@ -113,6 +113,11 @@ void uORB::AppsProtobufChannel::SubscribeCallback(const char *topic) test_flag = true; return; + } else if (strcmp(topic, "CPULOAD") == 0) { + // PX4_ERR("Got CPULOAD subscription"); + // This will happen when a newer PX4 version is talking to a + // SLPI image that doesn't support the CPULOAD request. If the + // SLPI image does support it then we wouldn't get this. } else if (_RxHandler) { pthread_mutex_lock(&_rx_mutex); _SlpiSubscriberCache[topic]++; diff --git a/src/modules/muorb/slpi/uORBProtobufChannel.cpp b/src/modules/muorb/slpi/uORBProtobufChannel.cpp index 64026bfa2fd8..d85fb65e488d 100644 --- a/src/modules/muorb/slpi/uORBProtobufChannel.cpp +++ b/src/modules/muorb/slpi/uORBProtobufChannel.cpp @@ -275,10 +275,10 @@ int px4muorb_orb_initialize(fc_func_ptrs *func_ptrs, int32_t clock_offset_us) uORB::Manager::get_instance()->set_uorb_communicator( uORB::ProtobufChannel::GetInstance()); - param_init(); - px4::WorkQueueManagerStart(); + param_init(); + uORB::ProtobufChannel::GetInstance()->RegisterSendHandler(muorb_func_ptrs.topic_data_func_ptr); // Configure the I2C driver function pointers @@ -385,8 +385,13 @@ int px4muorb_add_subscriber(const char *topic_name) uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler(); if (rxHandler) { - channel->AddRemoteSubscriber(topic_name); - // Pick a high message rate of 1000 Hz + if (channel->AddRemoteSubscriber(topic_name)) { + // Only process this subscription if it is the only one for the topic. + // Otherwise it will send some data from the queue and, most likely, + // mess up the queue on the remote side. + return 0; + } + return rxHandler->process_add_subscription(topic_name); } else { @@ -476,3 +481,32 @@ int px4muorb_send_topic_data(const char *topic_name, const uint8_t *data, return -1; } + + +float px4muorb_get_cpu_load(void) +{ + + // Default value to return if the SLPI code doesn't support + // queries for the CPU load + float cpu_load = 0.1f; + + uORB::ProtobufChannel *channel = uORB::ProtobufChannel::GetInstance(); + + if (channel) { + // The method to get the CPU load from the SLPI image is to send + // in the special code string to the add_subscription call. If it + // isn't supported the only return values can be 0 or -1. If it is + // supported then it will be some positive integer. + int16_t int_cpu_load = channel->add_subscription("CPULOAD", 0); + + if (int_cpu_load > 1) { + // Yay! CPU Load query is supported! + cpu_load = (float) int_cpu_load; + } + + } else { + PX4_ERR("Null channel pointer in %s", __FUNCTION__); + } + + return cpu_load; +} diff --git a/src/modules/muorb/slpi/uORBProtobufChannel.hpp b/src/modules/muorb/slpi/uORBProtobufChannel.hpp index ecd1d0278095..54ee99ed56c5 100644 --- a/src/modules/muorb/slpi/uORBProtobufChannel.hpp +++ b/src/modules/muorb/slpi/uORBProtobufChannel.hpp @@ -132,11 +132,13 @@ class uORB::ProtobufChannel : public uORBCommunicator::IChannel _Aggregator.RegisterSendHandler(func); } - void AddRemoteSubscriber(const std::string &messageName) + int AddRemoteSubscriber(const std::string &messageName) { + int currentRemoteSubscribers; pthread_mutex_lock(&_rx_mutex); - _AppsSubscriberCache[messageName]++; + currentRemoteSubscribers = _AppsSubscriberCache[messageName]++; pthread_mutex_unlock(&_rx_mutex); + return currentRemoteSubscribers; } void RemoveRemoteSubscriber(const std::string &messageName) @@ -214,6 +216,8 @@ extern "C" { int px4muorb_remove_subscriber(const char *name) __EXPORT; int px4muorb_send_topic_data(const char *name, const uint8_t *data, int data_len_in_bytes) __EXPORT; + + float px4muorb_get_cpu_load(void) __EXPORT; } #endif // _uORBProtobufChannel_hpp_ diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt index b996566defc2..c55cae3f07b9 100644 --- a/src/modules/navigator/CMakeLists.txt +++ b/src/modules/navigator/CMakeLists.txt @@ -69,4 +69,5 @@ px4_add_module( geofence_breach_avoidance motion_planning mission_feasibility_checker + rtl_time_estimator ) diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp index b31dacd9cef7..b27ef969aaeb 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp @@ -46,19 +46,11 @@ FeasibilityChecker::FeasibilityChecker() : void FeasibilityChecker::reset() { - - _is_landed = false; - _home_alt_msl = NAN; - _home_lat_lon = matrix::Vector2d((double)NAN, (double)NAN); - _current_position_lat_lon = matrix::Vector2d((double)NAN, (double)NAN); - _vehicle_type = VehicleType::RotaryWing; - _mission_validity_failed = false; _takeoff_failed = false; _land_pattern_validity_failed = false; _distance_first_waypoint_failed = false; _distance_between_waypoints_failed = false; - _below_home_alt_failed = false; _fixed_wing_land_approach_failed = false; _takeoff_land_available_failed = false; @@ -87,10 +79,16 @@ void FeasibilityChecker::updateData() if (home.valid_hpos) { _home_lat_lon = matrix::Vector2d(home.lat, home.lon); + + } else { + _home_lat_lon = matrix::Vector2d((double)NAN, (double)NAN); } if (home.valid_alt) { _home_alt_msl = home.alt; + + } else { + _home_alt_msl = NAN; } } @@ -126,6 +124,12 @@ void FeasibilityChecker::updateData() _current_position_lat_lon = matrix::Vector2d(vehicle_global_position.lat, vehicle_global_position.lon); } + if (_rtl_status_sub.updated()) { + rtl_status_s rtl_status = {}; + _rtl_status_sub.copy(&rtl_status); + _has_vtol_approach = rtl_status.has_vtol_approach; + } + param_t handle = param_find("FW_LND_ANG"); if (handle != PARAM_INVALID) { @@ -199,10 +203,6 @@ void FeasibilityChecker::doCommonChecks(mission_item_s &mission_item, const int _distance_first_waypoint_failed = !checkHorizontalDistanceToFirstWaypoint(mission_item); } - if (!_below_home_alt_failed) { - _below_home_alt_failed = !checkIfBelowHomeAltitude(mission_item, current_index); - } - if (!_takeoff_failed) { _takeoff_failed = !checkTakeoff(mission_item); } @@ -582,17 +582,22 @@ bool FeasibilityChecker::checkTakeoffLandAvailable() break; case 4: - result = _has_takeoff == _landing_valid; + result = hasMissionBothOrNeitherTakeoffAndLanding(); + + break; - if (!result && (_has_takeoff)) { - mavlink_log_critical(_mavlink_log_pub, "Mission rejected: Add Landing item or remove Takeoff.\t"); - events::send(events::ID("navigator_mis_add_land_or_rm_to"), {events::Log::Error, events::LogInternal::Info}, - "Mission rejected: Add Landing item or remove Takeoff"); + case 5: + if (!_is_landed && !_has_vtol_approach) { + result = _landing_valid; - } else if (!result && (_landing_valid)) { - mavlink_log_critical(_mavlink_log_pub, "Mission rejected: Add Takeoff item or remove Landing.\t"); - events::send(events::ID("navigator_mis_add_to_or_rm_land"), {events::Log::Error, events::LogInternal::Info}, - "Mission rejected: Add Takeoff item or remove Landing"); + if (!result) { + mavlink_log_critical(_mavlink_log_pub, "Mission rejected: Landing waypoint/pattern required."); + events::send(events::ID("feasibility_mis_in_air_landing_req"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: Landing waypoint/pattern required"); + } + + } else { + result = hasMissionBothOrNeitherTakeoffAndLanding(); } break; @@ -605,6 +610,23 @@ bool FeasibilityChecker::checkTakeoffLandAvailable() return result; } +bool FeasibilityChecker::hasMissionBothOrNeitherTakeoffAndLanding() +{ + bool result{_has_takeoff == _landing_valid}; + + if (!result && (_has_takeoff)) { + mavlink_log_critical(_mavlink_log_pub, "Mission rejected: Add Landing item or remove Takeoff.\t"); + events::send(events::ID("navigator_mis_add_land_or_rm_to"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: Add Landing item or remove Takeoff"); + + } else if (!result && (_landing_valid)) { + mavlink_log_critical(_mavlink_log_pub, "Mission rejected: Add Takeoff item or remove Landing.\t"); + events::send(events::ID("navigator_mis_add_to_or_rm_land"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: Add Takeoff item or remove Landing"); + } + + return result; +} bool FeasibilityChecker::checkHorizontalDistanceToFirstWaypoint(mission_item_s &mission_item) { @@ -682,21 +704,6 @@ bool FeasibilityChecker::checkDistancesBetweenWaypoints(const mission_item_s &mi return true; } -bool FeasibilityChecker::checkIfBelowHomeAltitude(const mission_item_s &mission_item, const int current_index) -{ - /* calculate the global waypoint altitude */ - float wp_alt = (mission_item.altitude_is_relative) ? mission_item.altitude + _home_alt_msl : mission_item.altitude; - - if (PX4_ISFINITE(_home_alt_msl) && _home_alt_msl > wp_alt && MissionBlock::item_contains_position(mission_item)) { - - mavlink_log_critical(_mavlink_log_pub, "Warning: Waypoint %d below home\t", current_index + 1); - events::send(events::ID("navigator_mis_wp_below_home"), {events::Log::Warning, events::LogInternal::Info}, - "Waypoint {1} below home", current_index + 1); - } - - return true; -} - bool FeasibilityChecker::checkItemsFitToVehicleType(const mission_item_s &mission_item) { if (_vehicle_type != VehicleType::Vtol && diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp index 493d618cce0c..fd18cbcfb68e 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp @@ -36,6 +36,7 @@ #include "../navigation.h" #include #include +#include #include #include #include @@ -81,7 +82,6 @@ class FeasibilityChecker : public ModuleParams _distance_between_waypoints_failed || _land_pattern_validity_failed || _fixed_wing_land_approach_failed || - _below_home_alt_failed || _mission_validity_failed || _takeoff_land_available_failed; } @@ -98,6 +98,7 @@ class FeasibilityChecker : public ModuleParams uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _land_detector_sub{ORB_ID(vehicle_land_detected)}; uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; + uORB::Subscription _rtl_status_sub{ORB_ID(rtl_status)}; // parameters float _param_fw_lnd_ang{0.f}; @@ -107,6 +108,7 @@ class FeasibilityChecker : public ModuleParams bool _is_landed{false}; float _home_alt_msl{NAN}; + bool _has_vtol_approach{false}; matrix::Vector2d _home_lat_lon = matrix::Vector2d((double)NAN, (double)NAN); matrix::Vector2d _current_position_lat_lon = matrix::Vector2d((double)NAN, (double)NAN); VehicleType _vehicle_type{VehicleType::RotaryWing}; @@ -117,7 +119,6 @@ class FeasibilityChecker : public ModuleParams bool _land_pattern_validity_failed{false}; bool _distance_first_waypoint_failed{false}; bool _distance_between_waypoints_failed{false}; - bool _below_home_alt_failed{false}; bool _fixed_wing_land_approach_failed{false}; bool _takeoff_land_available_failed{false}; bool _items_fit_to_vehicle_type_failed{false}; @@ -198,15 +199,6 @@ class FeasibilityChecker : public ModuleParams */ bool checkDistancesBetweenWaypoints(const mission_item_s &mission_item); - /** - * @brief Check if any waypoint is below the home altitude. Issues warning only. - * - * @param mission_item The current mission item - * @param current_index The current mission index - * @return Always returns true, only issues warning. - */ - bool checkIfBelowHomeAltitude(const mission_item_s &mission_item, const int current_index); - /** * @brief Check fixed wing land approach (fixed wing only) * @@ -258,4 +250,8 @@ class FeasibilityChecker : public ModuleParams * @return False if the check failed. */ void doMulticopterChecks(mission_item_s &mission_item, const int current_index); + + // Helper functions + + bool hasMissionBothOrNeitherTakeoffAndLanding(); }; diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp b/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp index fa5882690537..0de54f33766c 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp @@ -65,6 +65,18 @@ class TestFeasibilityChecker : public FeasibilityChecker orb_publish(ORB_ID(home_position), home_pub, &home); } + void publishInvalidHome() + { + home_position_s home = {}; + home.alt = 0.f; + home.valid_alt = false; + home.lat = 0.; + home.lon = 0.; + home.valid_hpos = false; + orb_advert_t home_pub = orb_advertise(ORB_ID(home_position), &home); + orb_publish(ORB_ID(home_position), home_pub, &home); + } + void publishCurrentPosition(double lat, double lon) { vehicle_global_position_s gpos = {}; @@ -122,6 +134,7 @@ TEST_F(FeasibilityCheckerTest, mission_item_validity) ASSERT_EQ(ret, false); checker.reset(); + checker.publishInvalidHome(); mission_item.nav_cmd = NAV_CMD_TAKEOFF; mission_item.altitude_is_relative = true; ret = checker.processNextItem(mission_item, 0, 5); @@ -190,6 +203,7 @@ TEST_F(FeasibilityCheckerTest, check_below_home) // this is done to invalidate the home position checker.reset(); + checker.publishInvalidHome(); checker.publishLanded(true); checker.processNextItem(mission_item, 0, 1); diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 5572d9b47422..55917331a614 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -81,6 +81,8 @@ Geofence::Geofence(Navigator *navigator) : if (_navigator != nullptr) { updateFence(); } + + _geofence_status_pub.advertise(); } Geofence::~Geofence() @@ -101,6 +103,14 @@ void Geofence::run() if (_initiate_fence_updated) { _initiate_fence_updated = false; _dataman_state = DatamanState::Read; + + geofence_status_s status; + status.timestamp = hrt_absolute_time(); + status.geofence_id = _opaque_id; + status.status = geofence_status_s::GF_STATUS_LOADING; + + _geofence_status_pub.publish(status); + } break; @@ -108,7 +118,7 @@ void Geofence::run() case DatamanState::Read: _dataman_state = DatamanState::ReadWait; - success = _dataman_client.readAsync(DM_KEY_FENCE_POINTS, 0, reinterpret_cast(&_stats), + success = _dataman_client.readAsync(DM_KEY_FENCE_POINTS_STATE, 0, reinterpret_cast(&_stats), sizeof(mission_stats_entry_s)); if (!success) { @@ -139,14 +149,22 @@ void Geofence::run() _dataman_cache.resize(_stats.num_items); } - for (int index = 1; index <= _dataman_cache.size(); ++index) { - _dataman_cache.load(DM_KEY_FENCE_POINTS, index); + for (int index = 0; index < _dataman_cache.size(); ++index) { + _dataman_cache.load(static_cast(_stats.dataman_id), index); } _dataman_state = DatamanState::Load; } else { _dataman_state = DatamanState::UpdateRequestWait; + _fence_updated = true; + + geofence_status_s status; + status.timestamp = hrt_absolute_time(); + status.geofence_id = _opaque_id; + status.status = geofence_status_s::GF_STATUS_READY; + + _geofence_status_pub.publish(status); } } @@ -160,6 +178,13 @@ void Geofence::run() _dataman_state = DatamanState::UpdateRequestWait; _updateFence(); _fence_updated = true; + + geofence_status_s status; + status.timestamp = hrt_absolute_time(); + status.geofence_id = _opaque_id; + status.status = geofence_status_s::GF_STATUS_READY; + + _geofence_status_pub.publish(status); } break; @@ -187,11 +212,11 @@ void Geofence::_updateFence() // iterate over all polygons and store their starting vertices _num_polygons = 0; - int current_seq = 1; + int current_seq = 0; - while (current_seq <= _dataman_cache.size()) { + while (current_seq < _dataman_cache.size()) { - bool success = _dataman_cache.loadWait(DM_KEY_FENCE_POINTS, current_seq, + bool success = _dataman_cache.loadWait(static_cast(_stats.dataman_id), current_seq, reinterpret_cast(&mission_fence_point), sizeof(mission_fence_point_s)); @@ -412,14 +437,15 @@ bool Geofence::insidePolygon(const PolygonInfo &polygon, double lat, double lon, for (unsigned i = 0, j = polygon.vertex_count - 1; i < polygon.vertex_count; j = i++) { - bool success = _dataman_cache.loadWait(DM_KEY_FENCE_POINTS, polygon.dataman_index + i, + dm_item_t fence_dataman_id{static_cast(_stats.dataman_id)}; + bool success = _dataman_cache.loadWait(fence_dataman_id, polygon.dataman_index + i, reinterpret_cast(&temp_vertex_i), sizeof(mission_fence_point_s)); if (!success) { break; } - success = _dataman_cache.loadWait(DM_KEY_FENCE_POINTS, polygon.dataman_index + j, + success = _dataman_cache.loadWait(fence_dataman_id, polygon.dataman_index + j, reinterpret_cast(&temp_vertex_j), sizeof(mission_fence_point_s)); if (!success) { @@ -448,7 +474,7 @@ 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, + bool success = _dataman_cache.loadWait(static_cast(_stats.dataman_id), polygon.dataman_index, reinterpret_cast(&circle_point), sizeof(mission_fence_point_s)); if (!success) { @@ -491,8 +517,18 @@ Geofence::loadFromFile(const char *filename) const char commentChar = '#'; int ret_val = PX4_ERROR; - /* Make sure no data is left in the datamanager */ - clearDm(); + mission_stats_entry_s stat; + { + const bool success = _dataman_client.readAsync(DM_KEY_FENCE_POINTS_STATE, 0, reinterpret_cast(&stat), + sizeof(mission_stats_entry_s)); + + if (!success) { + PX4_ERR("Could not read fence dataman state"); + return PX4_ERROR; + } + } + + dm_item_t write_fence_dataman_id{static_cast(stat.dataman_id) == DM_KEY_FENCE_POINTS_0 ? DM_KEY_FENCE_POINTS_1 : DM_KEY_FENCE_POINTS_0}; /* open the mixer definition file */ fp = fopen(GEOFENCE_FILENAME, "r"); @@ -554,7 +590,7 @@ Geofence::loadFromFile(const char *filename) } } - bool success = _dataman_client.writeSync(DM_KEY_FENCE_POINTS, pointCounter + 1, reinterpret_cast(&vertex), + bool success = _dataman_client.writeSync(write_fence_dataman_id, pointCounter, reinterpret_cast(&vertex), sizeof(vertex)); if (!success) { @@ -585,16 +621,16 @@ Geofence::loadFromFile(const char *filename) uint32_t crc32{0U}; /* do a second pass, now that we know the number of vertices */ - for (int seq = 1; seq <= pointCounter; ++seq) { + for (int seq = 0; seq < pointCounter; ++seq) { mission_fence_point_s mission_fence_point; - bool success = _dataman_client.readSync(DM_KEY_FENCE_POINTS, seq, reinterpret_cast(&mission_fence_point), + bool success = _dataman_client.readSync(write_fence_dataman_id, seq, reinterpret_cast(&mission_fence_point), sizeof(mission_fence_point_s)); if (success) { mission_fence_point.vertex_count = pointCounter; crc32 = crc32_for_fence_point(mission_fence_point, crc32); - _dataman_client.writeSync(DM_KEY_FENCE_POINTS, seq, reinterpret_cast(&mission_fence_point), + _dataman_client.writeSync(write_fence_dataman_id, seq, reinterpret_cast(&mission_fence_point), sizeof(mission_fence_point_s)); } } @@ -603,7 +639,7 @@ Geofence::loadFromFile(const char *filename) stats.num_items = pointCounter; stats.opaque_id = crc32; - bool success = _dataman_client.writeSync(DM_KEY_FENCE_POINTS, 0, reinterpret_cast(&stats), + bool success = _dataman_client.writeSync(DM_KEY_FENCE_POINTS_STATE, 0, reinterpret_cast(&stats), sizeof(mission_stats_entry_s)); if (success) { @@ -622,13 +658,6 @@ Geofence::loadFromFile(const char *filename) return ret_val; } -int Geofence::clearDm() -{ - _dataman_client.clearSync(DM_KEY_FENCE_POINTS); - updateFence(); - return PX4_OK; -} - bool Geofence::isHomeRequired() { bool max_horizontal_enabled = (_param_gf_max_hor_dist.get() > FLT_EPSILON); diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 3f2ecbba6167..71d0a6bdf7ec 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -49,6 +49,7 @@ #include #include #include +#include #include #include #include @@ -108,8 +109,6 @@ class Geofence : public ModuleParams virtual bool isInsidePolygonOrCircle(double lat, double lon, float altitude); - int clearDm(); - bool valid(); /** @@ -173,7 +172,7 @@ class Geofence : public ModuleParams mission_stats_entry_s _stats; DatamanState _dataman_state{DatamanState::UpdateRequestWait}; DatamanState _error_state{DatamanState::UpdateRequestWait}; - DatamanCache _dataman_cache{"geofence_dm_cache_miss", 4}; + DatamanCache _dataman_cache{"geofence_dm_cache_miss", 0}; DatamanClient &_dataman_client = _dataman_cache.client(); float _altitude_min{0.0f}; @@ -187,6 +186,8 @@ class Geofence : public ModuleParams 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 + uORB::Publication _geofence_status_pub{ORB_ID(geofence_status)}; + /** * implementation of updateFence() */ diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp index ad242d10a350..b6a951d1b766 100644 --- a/src/modules/navigator/land.cpp +++ b/src/modules/navigator/land.cpp @@ -83,7 +83,7 @@ Land::on_active() // create a virtual wp 1m in front of the vehicle to track during the backtransition waypoint_from_heading_and_distance(_navigator->get_global_position()->lat, _navigator->get_global_position()->lon, - _navigator->get_position_setpoint_triplet()->current.yaw, 1.f, + _navigator->get_local_position()->heading, 1.f, &pos_sp_triplet->current.lat, &pos_sp_triplet->current.lon); _navigator->set_position_setpoint_triplet_updated(); diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index cea25c30d86c..06b4151bdb9b 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -86,6 +86,8 @@ Mission::on_activation() { _need_mission_save = true; + check_mission_valid(true); + MissionBase::on_activation(); } @@ -192,11 +194,11 @@ void Mission::setActiveMissionItems() getNextPositionItems(_mission.current_seq + 1, next_mission_items_index, num_found_items, max_num_next_items); mission_item_s next_mission_items[max_num_next_items]; - const dm_item_t dataman_id = static_cast(_mission.dataman_id); + const dm_item_t mission_dataman_id = static_cast(_mission.mission_dataman_id); 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], + bool success = _dataman_cache.loadWait(mission_dataman_id, next_mission_items_index[i], reinterpret_cast(&next_mission_item), sizeof(next_mission_item), MAX_DATAMAN_LOAD_WAIT); if (success) { @@ -459,7 +461,7 @@ Mission::save_mission_state() 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.mission_dataman_id == _mission.mission_dataman_id && mission_state.count == _mission.count && mission_state.mission_id == _mission.mission_id) { /* navigator may modify only sequence, write modified state only if it changed */ if (mission_state.current_seq != _mission.current_seq) { diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp index 7fc5f6b5da72..026cefaba2c5 100644 --- a/src/modules/navigator/mission_base.cpp +++ b/src/modules/navigator/mission_base.cpp @@ -52,51 +52,34 @@ MissionBase::MissionBase(Navigator *navigator, int32_t dataman_cache_size_signed _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(); + // Reset _mission here, and listen on changes on the uorb topic instead of initialize from dataman. + _mission.mission_dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0; + _mission.fence_dataman_id = DM_KEY_FENCE_POINTS_0; + _mission.safepoint_dataman_id = DM_KEY_SAFE_POINTS_0; + _mission.count = 0; + _mission.current_seq = 0; + _mission.land_start_index = -1; + _mission.land_index = -1; + _mission.mission_id = 0; + _mission.geofence_id = 0; + _mission.safe_points_id = 0; _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)); + const int32_t start_index = math::constrain(_mission.current_seq, INT32_C(0), int32_t(_mission.count) - 1); + const int32_t end_index = math::constrain(start_index + _dataman_cache_size_signed, INT32_C(0), + int32_t(_mission.count) - 1); 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); + _dataman_cache.load(static_cast(_mission.mission_dataman_id), index); } _load_mission_index = _mission.current_seq; @@ -111,37 +94,43 @@ void MissionBase::updateMavlinkMission() 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_id != _mission.mission_id); + const bool mission_items_changed = (new_mission.mission_id != _mission.mission_id); + const bool mission_data_changed = checkMissionDataChanged(new_mission); - 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)); - } + if (new_mission.current_seq < 0) { + new_mission.current_seq = math::constrain(_mission.current_seq, INT32_C(0), + static_cast(new_mission.count) - 1); + } + + if (new_mission.geofence_id != _mission.geofence_id) { + // New geofence data, need to check mission again. + _mission_checked = false; + } + + _mission = new_mission; - _mission = new_mission; + /* Relevant mission items updated externally*/ + if (mission_data_changed) { - onMissionUpdate(mission_items_changed); - } + onMissionUpdate(mission_items_changed); } + + _is_current_planned_mission_item_valid = isMissionValid(); } } 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(); + if (canRunMissionFeasibility()) { + _mission_checked = true; + 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"); + } else { + _mission_checked = false; } } @@ -167,15 +156,17 @@ MissionBase::on_inactive() _land_detected_sub.update(); _vehicle_status_sub.update(); _global_pos_sub.update(); + _geofence_status_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 the mission */ + if (!_mission_checked && canRunMissionFeasibility()) { + _mission_checked = true; check_mission_valid(); - _initialized_mission_checked = true; + _is_current_planned_mission_item_valid = isMissionValid(); } if (_vehicle_status_sub.get().arming_state != vehicle_status_s::ARMING_STATE_ARMED) { @@ -216,8 +207,6 @@ MissionBase::on_activation() _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 @@ -258,11 +247,22 @@ MissionBase::on_active() _land_detected_sub.update(); _vehicle_status_sub.update(); _global_pos_sub.update(); + _geofence_status_sub.update(); parameters_update(); updateMavlinkMission(); updateDatamanCache(); + updateMissionAltAfterHomeChanged(); + + /* Check the mission */ + if (!_mission_checked && canRunMissionFeasibility()) { + _mission_checked = true; + check_mission_valid(); + _is_current_planned_mission_item_valid = isMissionValid(); + update_mission(); + set_mission_items(); + } // check if heading alignment is necessary, and add it to the current mission item if necessary if (_align_heading_necessary && is_mission_item_reached_or_completed()) { @@ -274,8 +274,8 @@ MissionBase::on_active() 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, + const dm_item_t mission_dataman_id = static_cast(_mission.mission_dataman_id); + bool success = _dataman_cache.loadWait(mission_dataman_id, next_mission_item_index, reinterpret_cast(&next_position_mission_item), sizeof(next_position_mission_item), MAX_DATAMAN_LOAD_WAIT); if (success) { @@ -460,7 +460,7 @@ MissionBase::set_mission_items() void MissionBase::loadCurrentMissionItem() { - const dm_item_t dm_item = static_cast(_mission.dataman_id); + const dm_item_t dm_item = static_cast(_mission.mission_dataman_id); bool success = _dataman_cache.loadWait(dm_item, _mission.current_seq, reinterpret_cast(&_mission_item), sizeof(mission_item_s), MAX_DATAMAN_LOAD_WAIT); @@ -574,10 +574,6 @@ void MissionBase::handleLanding(WorkItemType &new_work_item_type, mission_item_s set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC); 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) { @@ -681,7 +677,7 @@ MissionBase::checkMissionRestart() && ((_mission.current_seq + 1) == _mission.count)) { setMissionIndex(0); _inactivation_index = -1; // reset - _is_current_planned_mission_item_valid = isMissionValid(_mission); + _is_current_planned_mission_item_valid = isMissionValid(); resetMissionJumpCounter(); _navigator->reset_cruising_speed(); _navigator->reset_vroi(); @@ -690,11 +686,13 @@ MissionBase::checkMissionRestart() } void -MissionBase::check_mission_valid() +MissionBase::check_mission_valid(bool forced) { - if ((_navigator->get_mission_result()->mission_id != _mission.mission_id) - || (_navigator->get_mission_result()->geofence_id != _mission.geofence_id) - || (_navigator->get_mission_result()->home_position_counter != _navigator->get_home_position()->update_count)) { + // Allow forcing it, since we currently not rechecking if parameters have changed. + if (forced || + (_navigator->get_mission_result()->mission_id != _mission.mission_id) || + (_navigator->get_mission_result()->geofence_id != _mission.geofence_id) || + (_navigator->get_mission_result()->home_position_counter != _navigator->get_home_position()->update_count)) { _navigator->get_mission_result()->mission_id = _mission.mission_id; _navigator->get_mission_result()->geofence_id = _mission.geofence_id; @@ -707,6 +705,12 @@ MissionBase::check_mission_valid() _navigator->get_mission_result()->failure = false; set_mission_result(); + + // only warn if the check failed on merit + if ((!_navigator->get_mission_result()->valid) && _mission.count > 0U) { + PX4_WARN("mission check failed"); + } + } } @@ -833,7 +837,7 @@ MissionBase::do_abort_landing() } else { // move mission index back (landing approach point) - _is_current_planned_mission_item_valid = goToPreviousItem(false); + _is_current_planned_mission_item_valid = (goToPreviousItem(false) == PX4_OK); } // send reposition cmd to get out of mission @@ -879,15 +883,16 @@ void MissionBase::publish_navigator_mission_item() _navigator_mission_item_pub.publish(navigator_mission_item); } -bool MissionBase::isMissionValid(mission_s &mission) const +bool MissionBase::isMissionValid() 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)) { + if (((_mission.current_seq < _mission.count) || (_mission.count == 0U && _mission.current_seq <= 0)) && + (_mission.mission_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 || + _mission.mission_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1) && + (_mission.timestamp != 0u) && + (_navigator->get_mission_result()->valid)) { ret_val = true; - } return ret_val; @@ -900,13 +905,13 @@ int MissionBase::getNonJumpItem(int32_t &mission_index, mission_item_s &mission, return PX4_ERROR; } - const dm_item_t dataman_id = (dm_item_t)_mission.dataman_id; + const dm_item_t mission_dataman_id = (dm_item_t)_mission.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), + bool success = _dataman_cache.loadWait(mission_dataman_id, new_mission_index, reinterpret_cast(&new_mission), sizeof(mission_item_s), MAX_DATAMAN_LOAD_WAIT); if (!success) { @@ -926,7 +931,7 @@ int MissionBase::getNonJumpItem(int32_t &mission_index, mission_item_s &mission, 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), + success = _dataman_cache.writeWait(mission_dataman_id, new_mission_index, reinterpret_cast(&new_mission), sizeof(struct mission_item_s)); if (!success) { @@ -1099,12 +1104,12 @@ int MissionBase::setMissionToClosestItem(double lat, double lon, float alt, floa { 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); + const dm_item_t mission_dataman_id = static_cast(_mission.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), + bool success = _dataman_cache.loadWait(mission_dataman_id, mission_item_index, reinterpret_cast(&mission), sizeof(mission_item_s), MAX_DATAMAN_LOAD_WAIT); if (!success) { @@ -1143,26 +1148,25 @@ int MissionBase::setMissionToClosestItem(double lat, double lon, float alt, floa void MissionBase::resetMission() { /* we do not need to reset mission if is already.*/ - if (_mission.count == 0u && isMissionValid(_mission)) { + if (_mission.count == 0u) { 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_id = 0u; - 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), + _mission.timestamp = hrt_absolute_time(); + _mission.current_seq = 0; + _mission.land_start_index = -1; + _mission.land_index = -1; + _mission.count = 0u; + _mission.mission_id = 0u; + _mission.mission_dataman_id = _mission.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(&_mission), sizeof(mission_s)); if (success) { - _mission = new_mission; _mission_pub.publish(_mission); } else { @@ -1172,12 +1176,12 @@ void MissionBase::resetMission() void MissionBase::resetMissionJumpCounter() { - const dm_item_t dataman_id = static_cast(_mission.dataman_id); + const dm_item_t mission_dataman_id = static_cast(_mission.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), + bool success = _dataman_client.readSync(mission_dataman_id, mission_index, reinterpret_cast(&mission_item), sizeof(mission_item_s), MAX_DATAMAN_LOAD_WAIT); if (!success) { @@ -1191,7 +1195,8 @@ void MissionBase::resetMissionJumpCounter() 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), + bool write_success = _dataman_cache.writeWait(mission_dataman_id, mission_index, + reinterpret_cast(&mission_item), sizeof(struct mission_item_s)); if (!write_success) { @@ -1300,7 +1305,7 @@ 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; + const dm_item_t dm_current = (dm_item_t)_mission.mission_dataman_id; bool success = _dataman_client.readSync(dm_current, i, reinterpret_cast(&mission_item), sizeof(mission_item), 500_ms); @@ -1330,11 +1335,12 @@ void MissionBase::checkClimbRequired(int32_t mission_item_index) if (num_found_items > 0U) { - const dm_item_t dataman_id = static_cast(_mission.dataman_id); + const dm_item_t mission_dataman_id = static_cast(_mission.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), + const bool success = _dataman_cache.loadWait(mission_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 @@ -1357,7 +1363,42 @@ void MissionBase::checkClimbRequired(int32_t mission_item_index) bool MissionBase::checkMissionDataChanged(mission_s new_mission) { /* count and land_index are the same if the mission_id did not change. We do not care about changes in geofence or rally counters.*/ - return ((new_mission.dataman_id != _mission.dataman_id) || + return ((new_mission.mission_dataman_id != _mission.mission_dataman_id) || (new_mission.mission_id != _mission.mission_id) || (new_mission.current_seq != _mission.current_seq)); } + +bool MissionBase::canRunMissionFeasibility() +{ + return _navigator->home_global_position_valid() && // Need to have a home position checked + _navigator->get_global_position()->timestamp > 0 && // Need to have a position, for first waypoint check + (_geofence_status_sub.get().timestamp > 0) && // Geofence data must be loaded + (_geofence_status_sub.get().geofence_id == _mission.geofence_id) && + (_geofence_status_sub.get().status == geofence_status_s::GF_STATUS_READY); +} + +void MissionBase::updateMissionAltAfterHomeChanged() +{ + if (_navigator->get_home_position()->update_count > _home_update_counter) { + float new_alt = get_absolute_altitude_for_item(_mission_item); + float altitude_diff = new_alt - _navigator->get_position_setpoint_triplet()->current.alt; + + if (_navigator->get_position_setpoint_triplet()->previous.valid + && PX4_ISFINITE(_navigator->get_position_setpoint_triplet()->previous.alt)) { + _navigator->get_position_setpoint_triplet()->previous.alt = _navigator->get_position_setpoint_triplet()->previous.alt + + altitude_diff; + } + + _navigator->get_position_setpoint_triplet()->current.alt = _navigator->get_position_setpoint_triplet()->current.alt + + altitude_diff; + + if (_navigator->get_position_setpoint_triplet()->next.valid + && PX4_ISFINITE(_navigator->get_position_setpoint_triplet()->next.alt)) { + _navigator->get_position_setpoint_triplet()->next.alt = _navigator->get_position_setpoint_triplet()->next.alt + + altitude_diff; + } + + _navigator->set_position_setpoint_triplet_updated(); + _home_update_counter = _navigator->get_home_position()->update_count; + } +} diff --git a/src/modules/navigator/mission_base.h b/src/modules/navigator/mission_base.h index 660c918686f2..1b788106f4a3 100644 --- a/src/modules/navigator/mission_base.h +++ b/src/modules/navigator/mission_base.h @@ -43,6 +43,7 @@ #include #include #include +#include #include #include #include @@ -122,7 +123,7 @@ class MissionBase : public MissionBlock, public ModuleParams * @return true If mission has a land start of land item and a land item * @return false otherwise */ - bool hasMissionLandStart() const { return _mission.land_start_index > 0 && _mission.land_index > 0;}; + bool hasMissionLandStart() const { return _mission.land_start_index >= 0 && _mission.land_index >= 0;}; /** * @brief Go to next Mission Item * Go to next non jump mission item @@ -207,13 +208,18 @@ class MissionBase : public MissionBlock, public ModuleParams 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 + * @brief Is Mission Valid * - * @param mission Mission struct - * @return true is mission parameters are valid + * @return true is mission is valid * @return false otherwise */ - bool isMissionValid(mission_s &mission) const; + bool isMissionValid() const; + + /** + * @brief Check whether a mission is ready to go + * @param[in] forced flag if the check has to be run irregardles of any updates. + */ + void check_mission_valid(bool forced = false); /** * On mission update @@ -309,12 +315,15 @@ class MissionBase : public MissionBlock, public ModuleParams 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 _mission_checked{false}; /**< Flag indicating if the 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 */ 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. + 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*/ + DatamanCache _dataman_cache{"mission_dm_cache_miss", 10}; /**< Dataman cache of mission items*/ DatamanClient &_dataman_client = _dataman_cache.client(); /**< Dataman client*/ @@ -334,18 +343,13 @@ class MissionBase : public MissionBlock, public ModuleParams * @brief Update Dataman cache * */ - void updateDatamanCache(); + virtual void updateDatamanCache(); /** * @brief Update mission subscription * */ void updateMavlinkMission(); - /** - * Check whether a mission is ready to go - */ - void check_mission_valid(); - /** * Reset mission */ @@ -444,8 +448,15 @@ class MissionBase : public MissionBlock, public ModuleParams */ 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*/ + /** + * @brief update current mission altitude after the home position has changed. + */ + + void updateMissionAltAfterHomeChanged(); + + bool canRunMissionFeasibility(); + + uint32_t _home_update_counter = 0; /**< Variable to store the previous value for home change detection.*/ bool _align_heading_necessary{false}; // if true, heading of vehicle needs to be aligned with heading of next waypoint. Used to create new mission items for heading alignment. @@ -461,4 +472,5 @@ class MissionBase : public MissionBlock, public ModuleParams ) uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::SubscriptionData _geofence_status_sub{ORB_ID(geofence_status)}; }; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 27330190a019..17dd3e620ca2 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -110,7 +110,8 @@ MissionBlock::is_mission_item_reached_or_completed() if (int(_mission_item.params[0]) == 3) { // transition to RW requested, only accept waypoint if vehicle state has changed accordingly - return _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + return _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && !_navigator->get_vstatus()->in_transition_mode; } else if (int(_mission_item.params[0]) == 4) { // transition to FW requested, only accept waypoint if vehicle state has changed accordingly @@ -927,16 +928,15 @@ MissionBlock::initialize() _mission_item.origin = ORIGIN_ONBOARD; } -void MissionBlock::setLoiterToAltMissionItem(mission_item_s &item, const DestinationPosition &dest, float loiter_radius, - HeadingMode heading_mode) const +void MissionBlock::setLoiterToAltMissionItem(mission_item_s &item, const PositionYawSetpoint &pos_yaw_sp, + float loiter_radius) const { item.nav_cmd = NAV_CMD_LOITER_TO_ALT; - item.lat = dest.lat; - item.lon = dest.lon; - item.altitude = dest.alt; + item.lat = pos_yaw_sp.lat; + item.lon = pos_yaw_sp.lon; + item.altitude = pos_yaw_sp.alt; item.altitude_is_relative = false; - - item. yaw = setYawFromHeadingMode(dest, heading_mode); + item.yaw = pos_yaw_sp.yaw; item.acceptance_radius = _navigator->get_acceptance_radius(); item.time_inside = 0.0f; @@ -945,8 +945,8 @@ void MissionBlock::setLoiterToAltMissionItem(mission_item_s &item, const Destina item.loiter_radius = loiter_radius; } -void MissionBlock::setLoiterHoldMissionItem(mission_item_s &item, const DestinationPosition &dest, float loiter_time, - float loiter_radius, HeadingMode heading_mode) const +void MissionBlock::setLoiterHoldMissionItem(mission_item_s &item, const PositionYawSetpoint &pos_yaw_sp, + float loiter_time, float loiter_radius) const { const bool autocontinue = (loiter_time > -FLT_EPSILON); @@ -957,12 +957,12 @@ void MissionBlock::setLoiterHoldMissionItem(mission_item_s &item, const Destinat item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; } - item.lat = dest.lat; - item.lon = dest.lon; - item.altitude = dest.alt; + item.lat = pos_yaw_sp.lat; + item.lon = pos_yaw_sp.lon; + item.altitude = pos_yaw_sp.alt; item.altitude_is_relative = false; - item. yaw = setYawFromHeadingMode(dest, heading_mode); + item.yaw = NAN; item.acceptance_radius = _navigator->get_acceptance_radius(); item.time_inside = math::max(loiter_time, 0.0f); @@ -971,13 +971,12 @@ void MissionBlock::setLoiterHoldMissionItem(mission_item_s &item, const Destinat item.loiter_radius = loiter_radius; } -void MissionBlock::setMoveToPositionMissionItem(mission_item_s &item, const DestinationPosition &dest, - HeadingMode heading_mode) const +void MissionBlock::setMoveToPositionMissionItem(mission_item_s &item, const PositionYawSetpoint &pos_yaw_sp) const { item.nav_cmd = NAV_CMD_WAYPOINT; - item.lat = dest.lat; - item.lon = dest.lon; - item.altitude = dest.alt; + item.lat = pos_yaw_sp.lat; + item.lon = pos_yaw_sp.lon; + item.altitude = pos_yaw_sp.alt; item.altitude_is_relative = false; item.autocontinue = true; @@ -985,46 +984,22 @@ void MissionBlock::setMoveToPositionMissionItem(mission_item_s &item, const Dest item.time_inside = 0.f; item.origin = ORIGIN_ONBOARD; - item. yaw = setYawFromHeadingMode(dest, heading_mode); + item.yaw = pos_yaw_sp.yaw; } -void MissionBlock::setLandMissionItem(mission_item_s &item, const DestinationPosition &dest, - HeadingMode heading_mode) const +void MissionBlock::setLandMissionItem(mission_item_s &item, const PositionYawSetpoint &pos_yaw_sp) const { item.nav_cmd = NAV_CMD_LAND; - item.lat = dest.lat; - item.lon = dest.lon; - item.altitude = dest.alt; - - if (heading_mode == HeadingMode::CURRENT_HEADING) { - item.yaw = _navigator->get_local_position()->heading; - - } else { - item.yaw = dest.yaw; - } - + item.lat = pos_yaw_sp.lat; + item.lon = pos_yaw_sp.lon; + item.altitude = pos_yaw_sp.alt; + item.yaw = pos_yaw_sp.yaw; item.acceptance_radius = _navigator->get_acceptance_radius(); item.time_inside = 0.0f; item.autocontinue = true; item.origin = ORIGIN_ONBOARD; } -float MissionBlock::setYawFromHeadingMode(const DestinationPosition &dest, HeadingMode heading_mode) const -{ - float desired_yaw(_navigator->get_local_position()->heading); - - if (heading_mode == HeadingMode::NAVIGATION_HEADING) { - desired_yaw = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat, - _navigator->get_global_position()->lon, dest.lat, dest.lon); - - } else if (heading_mode == HeadingMode::DESTINATION_HEADING) { - desired_yaw = dest.yaw; - - } - - return desired_yaw; -} - void MissionBlock::startPrecLand(uint16_t land_precision) { if (_mission_item.land_precision == 1) { diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 619ea818df16..b97731567a29 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -205,18 +205,14 @@ class MissionBlock : public NavigatorMode */ void set_vtol_transition_item(struct mission_item_s *item, const uint8_t new_mode); - void setLoiterToAltMissionItem(mission_item_s &item, const DestinationPosition &dest, float loiter_radius, - HeadingMode heading_mode) const; + void setLoiterToAltMissionItem(mission_item_s &item, const PositionYawSetpoint &pos_yaw_sp, float loiter_radius) const; - void setLoiterHoldMissionItem(mission_item_s &item, const DestinationPosition &dest, float loiter_time, - float loiter_radius, HeadingMode heading_mode) const; + void setLoiterHoldMissionItem(mission_item_s &item, const PositionYawSetpoint &pos_yaw_sp, float loiter_time, + float loiter_radius) const; - void setMoveToPositionMissionItem(mission_item_s &item, const DestinationPosition &dest, - HeadingMode heading_mode) const; + void setMoveToPositionMissionItem(mission_item_s &item, const PositionYawSetpoint &pos_yaw_sp) const; - void setLandMissionItem(mission_item_s &item, const DestinationPosition &dest, HeadingMode heading_mode) const; - - float setYawFromHeadingMode(const DestinationPosition &dest, HeadingMode heading_mode) const; + void setLandMissionItem(mission_item_s &item, const PositionYawSetpoint &pos_yaw_sp) const; void startPrecLand(uint16_t land_precision); diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index b67a85224f06..dc2420d55bf5 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -79,7 +79,8 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission) for (size_t i = 0; i < mission.count; i++) { struct mission_item_s missionitem = {}; - bool success = _dataman_client.readSync((dm_item_t)mission.dataman_id, i, reinterpret_cast(&missionitem), + bool success = _dataman_client.readSync((dm_item_t)mission.mission_dataman_id, i, + reinterpret_cast(&missionitem), sizeof(mission_item_s)); if (!success) { @@ -119,7 +120,8 @@ MissionFeasibilityChecker::checkMissionAgainstGeofence(const mission_s &mission, for (size_t i = 0; i < mission.count; i++) { struct mission_item_s missionitem = {}; - bool success = _dataman_client.readSync((dm_item_t)mission.dataman_id, i, reinterpret_cast(&missionitem), + bool success = _dataman_client.readSync((dm_item_t)mission.mission_dataman_id, i, + reinterpret_cast(&missionitem), sizeof(mission_item_s)); if (!success) { diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index edfbac54bd5c..051f3c816b22 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -68,6 +68,7 @@ PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 2.5f); * @value 2 Require a landing * @value 3 Require a takeoff and a landing * @value 4 Require both a takeoff and a landing, or neither + * @value 5 Same as previous, but require a landing if in air and no valid VTOL landing approach is present * @group Mission */ PARAM_DEFINE_INT32(MIS_TKO_LAND_REQ, 0); diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index f9c27a7f2c57..87dd05d16675 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -200,17 +200,18 @@ struct mission_item_s { /** * dataman housekeeping information for a specific item. - * Corresponds to the first dataman entry of DM_KEY_FENCE_POINTS and DM_KEY_SAFE_POINTS + * Corresponds to the dataman entry of DM_KEY_FENCE_POINTS_STATE and DM_KEY_SAFE_POINTS_STATE */ struct mission_stats_entry_s { uint32_t opaque_id; /**< opaque identifier for current stored mission stats */ uint16_t num_items; /**< total number of items stored (excluding this one) */ - uint8_t padding[2]; + uint8_t dataman_id; /**< dm_item_t storage place*/ + uint8_t padding[1]; }; /** * Geofence vertex point. - * Corresponds to the DM_KEY_FENCE_POINTS dataman item + * Corresponds to the DM_KEY_FENCE_POINTS_0 dataman item */ struct mission_fence_point_s { double lat; @@ -229,15 +230,15 @@ struct mission_fence_point_s { }; /** - * @brief Return to launch position. - * Defines the position and landing yaw for the return to launch destination. + * @brief Position and yaw setpoint struct. + * Used in RTL state machine. * */ -struct DestinationPosition { - 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].*/ +struct PositionYawSetpoint { + double lat; /**< latitude setpoint in WGS84 [rad].*/ + double lon; /**< longitude setpoint in WGS84 [rad].*/ + float alt; /**< altitude setpoint in MSL [m].*/ + float yaw; /**< yaw setpoint [rad].*/ }; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 2215a2b23d71..5efcb45bf054 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -335,6 +335,8 @@ class Navigator : public ModuleBase, public ModuleParams GeofenceBreachAvoidance _gf_breach_avoidance; hrt_abstime _last_geofence_check = 0; + hrt_abstime _wait_for_vehicle_status_timestamp{0}; /**< If non-zero, wait for vehicle_status update before processing next cmd */ + bool _geofence_reposition_sent{false}; /**< flag if reposition command has been sent for current geofence breach*/ hrt_abstime _time_loitering_after_gf_breach{0}; /**< timestamp of when loitering after a geofence breach was started */ bool _pos_sp_triplet_updated{false}; /**< flags if position SP triplet needs to be published */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 1c8dbe42e347..8e2f24e1574c 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -208,7 +208,7 @@ void Navigator::run() if (mission.safe_points_id != safe_points_id) { safe_points_id = mission.safe_points_id; - _rtl.updateSafePoints(); + _rtl.updateSafePoints(safe_points_id); } } @@ -239,7 +239,8 @@ void Navigator::run() // Handle Vehicle commands int vehicle_command_updates = 0; - while (_vehicle_command_sub.updated() && (vehicle_command_updates < vehicle_command_s::ORB_QUEUE_LENGTH)) { + while (_wait_for_vehicle_status_timestamp == 0 && _vehicle_command_sub.updated() + && (vehicle_command_updates < vehicle_command_s::ORB_QUEUE_LENGTH)) { vehicle_command_updates++; const unsigned last_generation = _vehicle_command_sub.get_last_generation(); @@ -261,6 +262,9 @@ void Navigator::run() // only update the reposition setpoint if armed, as it otherwise won't get executed until the vehicle switches to loiter, // which can lead to dangerous and unexpected behaviors (see loiter.cpp, there is an if(armed) in there too) + // Wait for vehicle_status before handling the next command, otherwise the setpoint could be overwritten + _wait_for_vehicle_status_timestamp = hrt_absolute_time(); + vehicle_global_position_s position_setpoint{}; if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) { @@ -420,6 +424,9 @@ void Navigator::run() position_setpoint.lon = get_global_position()->lon; position_setpoint.alt = PX4_ISFINITE(cmd.param1) ? cmd.param1 : get_global_position()->alt; + // Wait for vehicle_status before handling the next command, otherwise the setpoint could be overwritten + _wait_for_vehicle_status_timestamp = hrt_absolute_time(); + if (geofence_allows_position(position_setpoint)) { position_setpoint_triplet_s *rep = get_reposition_triplet(); position_setpoint_triplet_s *curr = get_position_setpoint_triplet(); @@ -493,6 +500,9 @@ void Navigator::run() position_setpoint.lon = PX4_ISFINITE(cmd.param6) ? cmd.param6 : get_global_position()->lon; position_setpoint.alt = PX4_ISFINITE(cmd.param7) ? cmd.param7 : get_global_position()->alt; + // Wait for vehicle_status before handling the next command, otherwise the setpoint could be overwritten + _wait_for_vehicle_status_timestamp = hrt_absolute_time(); + if (geofence_allows_position(position_setpoint)) { position_setpoint_triplet_s *rep = get_reposition_triplet(); rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER; @@ -538,6 +548,9 @@ void Navigator::run() position_setpoint.lon = PX4_ISFINITE(cmd.param6) ? cmd.param6 : get_global_position()->lon; position_setpoint.alt = PX4_ISFINITE(cmd.param7) ? cmd.param7 : get_global_position()->alt; + // Wait for vehicle_status before handling the next command, otherwise the setpoint could be overwritten + _wait_for_vehicle_status_timestamp = hrt_absolute_time(); + if (geofence_allows_position(position_setpoint)) { position_setpoint_triplet_s *rep = get_reposition_triplet(); rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER; @@ -770,17 +783,7 @@ void Navigator::run() _pos_sp_triplet_published_invalid_once = false; } -#if CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF - - // If we are in VTOL takeoff, do not switch until it is finished. - if (_navigation_mode == &_vtol_takeoff && !get_mission_result()->finished) { - navigation_mode_new = &_vtol_takeoff; - - } else -#endif //CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF - { - navigation_mode_new = &_rtl; - } + navigation_mode_new = &_rtl; break; @@ -869,6 +872,10 @@ void Navigator::run() _navigation_mode = navigation_mode_new; + if (_wait_for_vehicle_status_timestamp != 0 && _vstatus.timestamp > _wait_for_vehicle_status_timestamp) { + _wait_for_vehicle_status_timestamp = 0; + } + /* iterate through navigation modes and set active/inactive for each */ for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) { if (_navigation_mode_array[i]) { diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 6bac232f34e2..bfa5d486be45 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -79,7 +79,7 @@ void RTL::updateDatamanCache() case DatamanState::Read: _dataman_state = DatamanState::ReadWait; - success = _dataman_client_safepoint.readAsync(DM_KEY_SAFE_POINTS, 0, reinterpret_cast(&_stats), + success = _dataman_client_safepoint.readAsync(DM_KEY_SAFE_POINTS_STATE, 0, reinterpret_cast(&_stats), sizeof(mission_stats_entry_s)); if (!success) { @@ -110,8 +110,8 @@ void RTL::updateDatamanCache() _dataman_cache_safepoint.resize(_stats.num_items); } - for (int index = 1; index <= _dataman_cache_safepoint.size(); ++index) { - _dataman_cache_safepoint.load(DM_KEY_SAFE_POINTS, index); + for (int index = 0; index < _dataman_cache_safepoint.size(); ++index) { + _dataman_cache_safepoint.load(static_cast(_stats.dataman_id), index); } _dataman_state = DatamanState::Load; @@ -146,7 +146,7 @@ void RTL::updateDatamanCache() if (_mission_id != _mission_sub.get().mission_id) { _mission_id = _mission_sub.get().mission_id; - const dm_item_t dm_item = static_cast(_mission_sub.get().dataman_id); + const dm_item_t dm_item = static_cast(_mission_sub.get().mission_dataman_id); _dataman_cache_landItem.invalidate(); if (_mission_sub.get().land_index > 0) { @@ -202,38 +202,43 @@ void RTL::on_inactive() break; } - // Limit inactive calculation to 1Hz + // Limit inactive calculation to 0.5Hz hrt_abstime now{hrt_absolute_time()}; - if ((now - _destination_check_time) > 1_s) { + if ((now - _destination_check_time) > 2_s) { _destination_check_time = now; setRtlTypeAndDestination(); + publishRemainingTimeEstimate(); + } - 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 estimated_time{}; - estimated_time.valid = false; +void RTL::publishRemainingTimeEstimate() +{ + const bool global_position_recently_updated = _global_pos_sub.get().timestamp > 0 + && hrt_elapsed_time(&_global_pos_sub.get().timestamp) < 10_s; - if (_navigator->home_global_position_valid() && global_position_recently_updated) { - switch (_rtl_type) { - case RtlType::RTL_DIRECT: - estimated_time = _rtl_direct.calc_rtl_time_estimate(); - break; + rtl_time_estimate_s estimated_time{}; + estimated_time.valid = false; - 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; + if (_navigator->home_global_position_valid() && global_position_recently_updated) { + switch (_rtl_type) { + case RtlType::RTL_DIRECT: + estimated_time = _rtl_direct.calc_rtl_time_estimate(); + break; - default: - break; - } - } + 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; - _rtl_time_estimate_pub.publish(estimated_time); + default: + break; + } } + + _rtl_time_estimate_pub.publish(estimated_time); } void RTL::on_activation() @@ -287,6 +292,14 @@ void RTL::on_active() default: break; } + + // Keep publishing remaining time estimates every 2 seconds + hrt_abstime now{hrt_absolute_time()}; + + if ((now - _destination_check_time) > 2_s) { + _destination_check_time = now; + publishRemainingTimeEstimate(); + } } void RTL::setRtlTypeAndDestination() @@ -294,12 +307,14 @@ void RTL::setRtlTypeAndDestination() init_rtl_mission_type(); + uint8_t safe_point_index{0U}; + if (_param_rtl_type.get() != 2) { // check the closest allowed destination. DestinationType destination_type{DestinationType::DESTINATION_TYPE_HOME}; - DestinationPosition rtl_position; + PositionYawSetpoint rtl_position; float rtl_alt; - findRtlDestination(destination_type, rtl_position, rtl_alt); + findRtlDestination(destination_type, rtl_position, rtl_alt, safe_point_index); switch (destination_type) { case DestinationType::DESTINATION_TYPE_MISSION_LAND: @@ -331,9 +346,29 @@ void RTL::setRtlTypeAndDestination() break; } } + + // Publish rtl status + _rtl_status_pub.get().timestamp = hrt_absolute_time(); + _rtl_status_pub.get().safe_points_id = _safe_points_id; + _rtl_status_pub.get().is_evaluation_pending = _dataman_state != DatamanState::UpdateRequestWait; + _rtl_status_pub.get().has_vtol_approach = false; + + if ((_param_rtl_type.get() == 0) || (_param_rtl_type.get() == 3)) { + _rtl_status_pub.get().has_vtol_approach = _home_has_land_approach || _one_rally_point_has_land_approach; + + } else if (_param_rtl_type.get() == 1) { + _rtl_status_pub.get().has_vtol_approach = _one_rally_point_has_land_approach; + } + + _rtl_status_pub.get().rtl_type = static_cast(_rtl_type); + _rtl_status_pub.get().safe_point_index = safe_point_index; + + _rtl_status_pub.update(); + } -void RTL::findRtlDestination(DestinationType &destination_type, DestinationPosition &rtl_position, float &rtl_alt) +void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpoint &rtl_position, float &rtl_alt, + uint8_t &safe_point_index) { // set destination to home per default, then check if other valid landing spot is closer rtl_position.alt = _home_pos_sub.get().alt; @@ -352,8 +387,10 @@ void RTL::findRtlDestination(DestinationType &destination_type, DestinationPosit 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; + _home_has_land_approach = hasVtolLandApproach(rtl_position); + if (((_param_rtl_type.get() == 1) && !vtol_in_rw_mode) || (vtol_in_fw_mode && (_param_rtl_approach_force.get() == 1) - && !hasVtolLandApproach(rtl_position))) { + && !_home_has_land_approach)) { // Set minimum distance to maximum value when RTL_TYPE is set to 1 and we are not in RW mode or we forces approach landing for vtol in fw and it is not defined for home. min_dist = FLT_MAX; @@ -365,7 +402,7 @@ void RTL::findRtlDestination(DestinationType &destination_type, DestinationPosit if (((_param_rtl_type.get() == 1) || (_param_rtl_type.get() == 3) || (fabsf(FLT_MAX - min_dist) < FLT_EPSILON)) && hasMissionLandStart()) { mission_item_s land_mission_item; - const dm_item_t dm_item = static_cast(_mission_sub.get().dataman_id); + const dm_item_t dm_item = static_cast(_mission_sub.get().mission_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); @@ -394,10 +431,12 @@ void RTL::findRtlDestination(DestinationType &destination_type, DestinationPosit if (_safe_points_updated) { - for (int current_seq = 1; current_seq <= _dataman_cache_safepoint.size(); ++current_seq) { + _one_rally_point_has_land_approach = false; + + for (int current_seq = 0; current_seq < _dataman_cache_safepoint.size(); ++current_seq) { mission_item_s mission_safe_point; - bool success = _dataman_cache_safepoint.loadWait(DM_KEY_SAFE_POINTS, current_seq, + bool success = _dataman_cache_safepoint.loadWait(static_cast(_stats.dataman_id), current_seq, reinterpret_cast(&mission_safe_point), sizeof(mission_item_s), 500_ms); @@ -413,14 +452,19 @@ void RTL::findRtlDestination(DestinationType &destination_type, DestinationPosit if (mission_safe_point.nav_cmd == NAV_CMD_RALLY_POINT && dist_to_home > MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES) { 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)}; - DestinationPosition safepoint_position; + PositionYawSetpoint safepoint_position; setSafepointAsDestination(safepoint_position, mission_safe_point); + bool current_safe_point_has_approaches{hasVtolLandApproach(safepoint_position)}; + + _one_rally_point_has_land_approach |= current_safe_point_has_approaches; + if (((dist + MIN_DIST_THRESHOLD) < min_dist) && (!vtol_in_fw_mode || (_param_rtl_approach_force.get() == 0) - || hasVtolLandApproach(safepoint_position))) { + || current_safe_point_has_approaches)) { min_dist = dist; rtl_position = safepoint_position; destination_type = DestinationType::DESTINATION_TYPE_SAFE_POINT; + safe_point_index = current_seq; } } } @@ -435,7 +479,7 @@ void RTL::findRtlDestination(DestinationType &destination_type, DestinationPosit } } -void RTL::setLandPosAsDestination(DestinationPosition &rtl_position, mission_item_s &land_mission_item) const +void RTL::setLandPosAsDestination(PositionYawSetpoint &rtl_position, mission_item_s &land_mission_item) const { rtl_position.alt = land_mission_item.altitude_is_relative ? land_mission_item.altitude + _home_pos_sub.get().alt : land_mission_item.altitude; @@ -444,7 +488,7 @@ void RTL::setLandPosAsDestination(DestinationPosition &rtl_position, mission_ite rtl_position.yaw = _home_pos_sub.get().yaw; } -void RTL::setSafepointAsDestination(DestinationPosition &rtl_position, +void RTL::setSafepointAsDestination(PositionYawSetpoint &rtl_position, const mission_item_s &mission_safe_point) const { // There is a safe point closer than home/mission landing @@ -472,7 +516,7 @@ void RTL::setSafepointAsDestination(DestinationPosition &rtl_position, } } -float RTL::calculate_return_alt_from_cone_half_angle(const DestinationPosition &rtl_position, +float RTL::calculate_return_alt_from_cone_half_angle(const PositionYawSetpoint &rtl_position, float cone_half_angle_deg) const { // horizontal distance to destination @@ -573,10 +617,10 @@ void RTL::parameters_update() bool RTL::hasMissionLandStart() const { - return _mission_sub.get().land_start_index > 0; + return _mission_sub.get().land_start_index >= 0 && _mission_sub.get().land_index >= 0; } -bool RTL::hasVtolLandApproach(const DestinationPosition &rtl_position) const +bool RTL::hasVtolLandApproach(const PositionYawSetpoint &rtl_position) const { return readVtolLandApproaches(rtl_position).isAnyApproachValid(); } @@ -611,7 +655,7 @@ loiter_point_s RTL::chooseBestLandingApproach(const land_approaches_s &vtol_land } } -land_approaches_s RTL::readVtolLandApproaches(DestinationPosition rtl_position) const +land_approaches_s RTL::readVtolLandApproaches(PositionYawSetpoint rtl_position) const { // go through all mission items in the rally point storage. If we find a mission item of type NAV_CMD_RALLY_POINT @@ -626,10 +670,10 @@ land_approaches_s RTL::readVtolLandApproaches(DestinationPosition rtl_position) bool foundHomeLandApproaches = false; uint8_t sector_counter = 0; - for (int current_seq = 1; current_seq <= _stats.num_items; ++current_seq) { + for (int current_seq = 0; current_seq < _stats.num_items; ++current_seq) { mission_item_s mission_item{}; - bool success_mission_item = _dataman_cache_safepoint.loadWait(DM_KEY_SAFE_POINTS, current_seq, + bool success_mission_item = _dataman_cache_safepoint.loadWait(static_cast(_stats.dataman_id), current_seq, reinterpret_cast(&mission_item), sizeof(mission_item_s)); diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 9ebc7ab4e305..ac6a5a75d774 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -58,6 +58,7 @@ #include #include #include +#include #include class Navigator; @@ -86,7 +87,7 @@ class RTL : public NavigatorMode, public ModuleParams void set_return_alt_min(bool min) { _enforce_rtl_alt = min; } - void updateSafePoints() { _initiate_safe_points_updated = true; } + void updateSafePoints(uint32_t new_safe_point_id) { _initiate_safe_points_updated = true; _safe_points_id = new_safe_point_id; } private: enum class DestinationType { @@ -105,24 +106,31 @@ class RTL : public NavigatorMode, public ModuleParams void setRtlTypeAndDestination(); + /** + * @brief Publish the remaining time estimate to go to the RTL landing point. + * + */ + void publishRemainingTimeEstimate(); + /** * @brief Find RTL destination. * */ - void findRtlDestination(DestinationType &destination_type, DestinationPosition &rtl_position, float &rtl_alt); + void findRtlDestination(DestinationType &destination_type, PositionYawSetpoint &rtl_position, float &rtl_alt, + uint8_t &safe_point_index); /** * @brief Set the position of the land start marker in the planned mission as destination. * */ - void setLandPosAsDestination(DestinationPosition &rtl_position, mission_item_s &land_mission_item) const; + void setLandPosAsDestination(PositionYawSetpoint &rtl_position, mission_item_s &land_mission_item) const; /** * @brief Set the safepoint as destination. * * @param mission_safe_point is the mission safe point/rally point to set as destination. */ - void setSafepointAsDestination(DestinationPosition &rtl_position, const mission_item_s &mission_safe_point) const; + void setSafepointAsDestination(PositionYawSetpoint &rtl_position, const mission_item_s &mission_safe_point) const; /** * @brief calculate return altitude from cone half angle @@ -131,7 +139,7 @@ class RTL : public NavigatorMode, public ModuleParams * @param[in] cone_half_angle_deg half angle of the cone [deg] * @return return altitude */ - float calculate_return_alt_from_cone_half_angle(const DestinationPosition &rtl_position, + float calculate_return_alt_from_cone_half_angle(const PositionYawSetpoint &rtl_position, float cone_half_angle_deg) const; /** @@ -152,7 +160,7 @@ class RTL : public NavigatorMode, public ModuleParams * @param[in] rtl_position landing position of the rtl * */ - land_approaches_s readVtolLandApproaches(DestinationPosition rtl_position) const; + land_approaches_s readVtolLandApproaches(PositionYawSetpoint rtl_position) const; /** * @brief Has VTOL land approach @@ -162,7 +170,7 @@ class RTL : public NavigatorMode, public ModuleParams * @return true if home land approaches are defined for home position * @return false otherwise */ - bool hasVtolLandApproach(const DestinationPosition &rtl_position) const; + bool hasVtolLandApproach(const PositionYawSetpoint &rtl_position) const; /** * @brief Choose best landing approach @@ -188,6 +196,9 @@ class RTL : public NavigatorMode, public ModuleParams RtlType _rtl_type{RtlType::RTL_DIRECT}; + bool _home_has_land_approach; ///< Flag if the home position has a land approach defined + bool _one_rally_point_has_land_approach; ///< Flag if a rally point has a land approach defined + DatamanState _dataman_state{DatamanState::UpdateRequestWait}; DatamanState _error_state{DatamanState::UpdateRequestWait}; uint32_t _opaque_id{0}; ///< dataman safepoint id: if it does not match, safe points data was updated @@ -197,6 +208,7 @@ class RTL : public NavigatorMode, public ModuleParams bool _initiate_safe_points_updated{true}; ///< flag indicating if safe points update is needed mutable DatamanCache _dataman_cache_landItem{"rtl_dm_cache_miss_land", 2}; uint32_t _mission_id = 0u; + uint32_t _safe_points_id = 0u; mission_stats_entry_s _stats; @@ -222,4 +234,5 @@ class RTL : public NavigatorMode, public ModuleParams uORB::SubscriptionData _wind_sub{ORB_ID(wind)}; uORB::Publication _rtl_time_estimate_pub{ORB_ID(rtl_time_estimate)}; + uORB::PublicationData _rtl_status_pub{ORB_ID(rtl_status)}; }; diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp index 63c876b3c5f8..d2936986db9a 100644 --- a/src/modules/navigator/rtl_direct.cpp +++ b/src/modules/navigator/rtl_direct.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2024 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -59,15 +59,6 @@ RtlDirect::RtlDirect(Navigator *navigator) : _land_approach.lat = static_cast(NAN); _land_approach.lon = static_cast(NAN); _land_approach.height_m = NAN; - - _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() @@ -122,7 +113,7 @@ void RtlDirect::on_active() } } -void RtlDirect::setRtlPosition(DestinationPosition rtl_position, loiter_point_s loiter_pos) +void RtlDirect::setRtlPosition(PositionYawSetpoint rtl_position, loiter_point_s loiter_pos) { _home_pos_sub.update(); @@ -130,7 +121,6 @@ void RtlDirect::setRtlPosition(DestinationPosition rtl_position, loiter_point_s // Only allow to set a new approach if the mode is not activated yet. if (!isActive()) { - _land_approach = loiter_pos; _destination = rtl_position; _force_heading = false; @@ -148,24 +138,12 @@ void RtlDirect::setRtlPosition(DestinationPosition rtl_position, loiter_point_s _destination.alt = _home_pos_sub.get().alt; } - if (!PX4_ISFINITE(_land_approach.lat) || !PX4_ISFINITE(_land_approach.lon)) { - _land_approach.lat = _destination.lat; - _land_approach.lon = _destination.lon; - - } else { - const float dist_to_destination{get_distance_to_next_waypoint(_land_approach.lat, _land_approach.lon, _destination.lat, _destination.lon)}; - - if (dist_to_destination > _navigator->get_acceptance_radius()) { - _force_heading = true; - } - } + _land_approach = sanitizeLandApproach(loiter_pos); - if (!PX4_ISFINITE(_land_approach.height_m)) { - _land_approach.height_m = _destination.alt + _param_rtl_descend_alt.get(); - } + const float dist_to_destination{get_distance_to_next_waypoint(_land_approach.lat, _land_approach.lon, _destination.lat, _destination.lon)}; - if (!PX4_ISFINITE(_land_approach.loiter_radius_m) || fabsf(_land_approach.loiter_radius_m) <= FLT_EPSILON) { - _land_approach.loiter_radius_m = _param_rtl_loiter_rad.get(); + if (dist_to_destination > _navigator->get_acceptance_radius()) { + _force_heading = true; } } } @@ -178,41 +156,39 @@ void RtlDirect::set_rtl_item() _global_pos_sub.get().lat, _global_pos_sub.get().lon); const float loiter_altitude = math::min(_land_approach.height_m, _rtl_alt); - HeadingMode rtl_heading_mode = static_cast(_param_rtl_hdg_md.get()); - - if ((rtl_heading_mode == HeadingMode::NAVIGATION_HEADING) && (destination_dist < _param_rtl_min_dist.get())) { - rtl_heading_mode = HeadingMode::DESTINATION_HEADING; - } + const bool is_close_to_destination = destination_dist < _param_rtl_min_dist.get(); switch (_rtl_state) { case RTLState::CLIMBING: { - DestinationPosition dest { + PositionYawSetpoint pos_yaw_sp { .lat = _global_pos_sub.get().lat, .lon = _global_pos_sub.get().lon, .alt = _rtl_alt, + .yaw = _param_wv_en.get() ? NAN : _navigator->get_local_position()->heading, }; - - setLoiterToAltMissionItem(_mission_item, dest, _navigator->get_loiter_radius(), HeadingMode::CURRENT_HEADING); + setLoiterToAltMissionItem(_mission_item, pos_yaw_sp, _navigator->get_loiter_radius()); _rtl_state = RTLState::MOVE_TO_LOITER; break; } case RTLState::MOVE_TO_LOITER: { - DestinationPosition dest { + PositionYawSetpoint pos_yaw_sp { .lat = _land_approach.lat, .lon = _land_approach.lon, .alt = _rtl_alt, - .yaw = _destination.yaw, }; // 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) { - setLoiterHoldMissionItem(_mission_item, dest, 0.f, _land_approach.loiter_radius_m, rtl_heading_mode); + pos_yaw_sp.yaw = NAN; + setLoiterHoldMissionItem(_mission_item, pos_yaw_sp, 0.f, _land_approach.loiter_radius_m); } else { - setMoveToPositionMissionItem(_mission_item, dest, rtl_heading_mode); + // already set final yaw if close to destination and weather vane is disabled + pos_yaw_sp.yaw = (is_close_to_destination && !_param_wv_en.get()) ? _destination.yaw : NAN; + setMoveToPositionMissionItem(_mission_item, pos_yaw_sp); } _rtl_state = RTLState::LOITER_DOWN; @@ -221,14 +197,14 @@ void RtlDirect::set_rtl_item() } case RTLState::LOITER_DOWN: { - DestinationPosition dest{ + PositionYawSetpoint pos_yaw_sp{ .lat = _land_approach.lat, .lon = _land_approach.lon, .alt = loiter_altitude, - .yaw = _destination.yaw, + .yaw = !_param_wv_en.get() ? _destination.yaw : NAN, // set final yaw if weather vane is disabled }; - setLoiterToAltMissionItem(_mission_item, dest, _land_approach.loiter_radius_m, rtl_heading_mode); + setLoiterToAltMissionItem(_mission_item, pos_yaw_sp, _land_approach.loiter_radius_m); pos_sp_triplet->next.valid = true; pos_sp_triplet->next.lat = _destination.lat; @@ -248,15 +224,14 @@ void RtlDirect::set_rtl_item() } case RTLState::LOITER_HOLD: { - DestinationPosition dest { + PositionYawSetpoint pos_yaw_sp { .lat = _land_approach.lat, .lon = _land_approach.lon, .alt = loiter_altitude, - .yaw = _destination.yaw, + .yaw = !_param_wv_en.get() ? _destination.yaw : NAN, // set final yaw if weather vane is disabled }; - setLoiterHoldMissionItem(_mission_item, dest, _param_rtl_land_delay.get(), _land_approach.loiter_radius_m, - rtl_heading_mode); + setLoiterHoldMissionItem(_mission_item, pos_yaw_sp, _param_rtl_land_delay.get(), _land_approach.loiter_radius_m); if (_param_rtl_land_delay.get() < -FLT_EPSILON) { mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loitering\t"); @@ -276,10 +251,11 @@ void RtlDirect::set_rtl_item() case RTLState::MOVE_TO_LAND: { - DestinationPosition dest{_destination}; - dest.alt = loiter_altitude; + PositionYawSetpoint pos_yaw_sp{_destination}; + pos_yaw_sp.alt = loiter_altitude; + pos_yaw_sp.yaw = NAN; - setMoveToPositionMissionItem(_mission_item, dest, rtl_heading_mode); + setMoveToPositionMissionItem(_mission_item, pos_yaw_sp); // Prepare for transition _mission_item.vtol_back_transition = true; @@ -289,7 +265,7 @@ void RtlDirect::set_rtl_item() // location and land location after exiting the loiter circle pos_sp_triplet->previous.lat = _land_approach.lat; pos_sp_triplet->previous.lon = _land_approach.lon; - pos_sp_triplet->previous.alt = _mission_item.altitude; + pos_sp_triplet->previous.alt = get_absolute_altitude_for_item(_mission_item); pos_sp_triplet->previous.valid = true; _rtl_state = RTLState::TRANSITION_TO_MC; @@ -306,10 +282,12 @@ void RtlDirect::set_rtl_item() } case RTLState::MOVE_TO_LAND_HOVER: { - DestinationPosition dest{_destination}; - dest.alt = loiter_altitude; + PositionYawSetpoint pos_yaw_sp{_destination}; + pos_yaw_sp.alt = loiter_altitude; + pos_yaw_sp.yaw = !_param_wv_en.get() ? _destination.yaw : NAN; // set final yaw if weather vane is disabled - setMoveToPositionMissionItem(_mission_item, dest, rtl_heading_mode); + setMoveToPositionMissionItem(_mission_item, pos_yaw_sp); + _navigator->reset_position_setpoint(pos_sp_triplet->previous); _rtl_state = RTLState::LAND; @@ -317,8 +295,9 @@ void RtlDirect::set_rtl_item() } case RTLState::LAND: { - - setLandMissionItem(_mission_item, _destination, rtl_heading_mode); + PositionYawSetpoint pos_yaw_sp{_destination}; + pos_yaw_sp.yaw = !_param_wv_en.get() ? _destination.yaw : NAN; // set final yaw if weather vane is disabled + setLandMissionItem(_mission_item, pos_yaw_sp); _mission_item.land_precision = _param_rtl_pld_md.get(); @@ -378,8 +357,9 @@ RtlDirect::RTLState RtlDirect::getActivationLandState() rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate() { _global_pos_sub.update(); + _rtl_time_estimator.update(); - rtl_time_estimate_s rtl_time_estimate{}; + _rtl_time_estimator.reset(); RTLState start_state_for_estimate; @@ -390,34 +370,31 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate() start_state_for_estimate = getActivationLandState(); } - // Calculate RTL time estimate only when there is a valid home position + // Calculate RTL time estimate only when there is a valid destination // TODO: Also check if vehicle position is valid - if (!_navigator->home_global_position_valid()) { - rtl_time_estimate.valid = false; + if (PX4_ISFINITE(_destination.lat) && PX4_ISFINITE(_destination.lon) && PX4_ISFINITE(_destination.alt)) { - } else { - rtl_time_estimate.valid = true; - rtl_time_estimate.time_estimate = 0.f; + loiter_point_s land_approach = sanitizeLandApproach(_land_approach); - const float loiter_altitude = min(_land_approach.height_m, _rtl_alt); + const float loiter_altitude = min(land_approach.height_m, _rtl_alt); // Sum up time estimate for various segments of the landing procedure switch (start_state_for_estimate) { case RTLState::CLIMBING: { // 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(); + if ((_global_pos_sub.get().alt < _rtl_alt) || _enforce_rtl_alt) { + _rtl_time_estimator.addVertDistance(_rtl_alt - _global_pos_sub.get().alt); } } // FALLTHROUGH - case RTLState::MOVE_TO_LOITER: - - // Add cruise segment to home - rtl_time_estimate.time_estimate += get_distance_to_next_waypoint( - _land_approach.lat, _land_approach.lon, _global_pos_sub.get().lat, _global_pos_sub.get().lon) / getCruiseGroundSpeed(); + case RTLState::MOVE_TO_LOITER: { + matrix::Vector2f direction{}; + get_vector_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, land_approach.lat, + land_approach.lon, &direction(0), &direction(1)); + _rtl_time_estimator.addDistance(get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, + land_approach.lat, land_approach.lon), direction, 0.f); + } // FALLTHROUGH case RTLState::LOITER_DOWN: { @@ -433,15 +410,18 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate() initial_altitude = _rtl_alt; // CLIMB and RETURN } - // Add descend segment (first landing phase: return alt to loiter alt) - rtl_time_estimate.time_estimate += fabsf(initial_altitude - loiter_altitude) / getDescendRate(); + _rtl_time_estimator.addVertDistance(loiter_altitude - initial_altitude); } // FALLTHROUGH case RTLState::LOITER_HOLD: // 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(); + _rtl_time_estimator.addWait(_param_rtl_land_delay.get()); + + if (_param_rtl_land_delay.get() < -FLT_EPSILON) { // Set to loiter infinitely and not land. Stop calculation here + break; + } + // FALLTHROUGH case RTLState::MOVE_TO_LAND: @@ -449,19 +429,22 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate() case RTLState::MOVE_TO_LAND_HOVER: { // Add cruise segment to home float move_to_land_dist{0.f}; + matrix::Vector2f direction{}; if (start_state_for_estimate >= RTLState::MOVE_TO_LAND) { move_to_land_dist = get_distance_to_next_waypoint( - _destination.lat, _destination.lon, _global_pos_sub.get().lat, _global_pos_sub.get().lon); + _global_pos_sub.get().lat, _global_pos_sub.get().lon, _destination.lat, _destination.lon); + get_vector_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, _destination.lat, _destination.lon, + &direction(0), &direction(1)); } else { move_to_land_dist = get_distance_to_next_waypoint( - _destination.lat, _destination.lon, _land_approach.lat, _land_approach.lon); + land_approach.lat, land_approach.lon, _destination.lat, _destination.lon); + get_vector_to_next_waypoint(land_approach.lat, land_approach.lon, _destination.lat, _destination.lon, &direction(0), + &direction(1)); } - if (move_to_land_dist > FLT_EPSILON) { - rtl_time_estimate.time_estimate += move_to_land_dist / getCruiseGroundSpeed(); - } + _rtl_time_estimator.addDistance(move_to_land_dist, direction, 0.f); } // FALLTHROUGH @@ -481,10 +464,7 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate() 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(); - } + _rtl_time_estimator.addDescendMCLand(_destination.alt - initial_altitude); } break; @@ -493,142 +473,40 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate() // 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; + return _rtl_time_estimator.getEstimate(); } -matrix::Vector2f RtlDirect::get_wind() +void RtlDirect::parameters_update() { - _wind_sub.update(); - matrix::Vector2f wind; + if (_parameter_update_sub.updated()) { + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); - if (hrt_absolute_time() - _wind_sub.get().timestamp < 1_s) { - wind(0) = _wind_sub.get().windspeed_north; - wind(1) = _wind_sub.get().windspeed_east; + // If any parameter updated, call updateParams() to check if + // this class attributes need updating (and do so). + updateParams(); } - - return wind; } -float RtlDirect::getClimbRate() +loiter_point_s RtlDirect::sanitizeLandApproach(loiter_point_s land_approach) const { - float ret = 1e6f; + loiter_point_s sanitized_land_approach{land_approach}; - 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; - } + if (!PX4_ISFINITE(land_approach.lat) || !PX4_ISFINITE(land_approach.lon)) { + sanitized_land_approach.lat = _destination.lat; + sanitized_land_approach.lon = _destination.lon; - } 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; - } + if (!PX4_ISFINITE(land_approach.height_m)) { + sanitized_land_approach.height_m = _destination.alt + _param_rtl_descend_alt.get(); } - 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; + if (!PX4_ISFINITE(land_approach.loiter_radius_m) || fabsf(land_approach.loiter_radius_m) <= FLT_EPSILON) { + sanitized_land_approach.loiter_radius_m = _param_rtl_loiter_rad.get(); } - 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(); - } + return sanitized_land_approach; } diff --git a/src/modules/navigator/rtl_direct.h b/src/modules/navigator/rtl_direct.h index 3a5597d4ad52..211b2779eae0 100644 --- a/src/modules/navigator/rtl_direct.h +++ b/src/modules/navigator/rtl_direct.h @@ -54,6 +54,7 @@ #include #include +#include #include "mission_block.h" #include "navigation.h" #include "safe_point_land.hpp" @@ -99,7 +100,7 @@ class RtlDirect : public MissionBlock, public ModuleParams void setReturnAltMin(bool min) { _enforce_rtl_alt = min; } void setRtlAlt(float alt) {_rtl_alt = alt;}; - void setRtlPosition(DestinationPosition position, loiter_point_s loiter_pos); + void setRtlPosition(PositionYawSetpoint position, loiter_point_s loiter_pos); private: /** @@ -119,13 +120,6 @@ class RtlDirect : public MissionBlock, public ModuleParams } _rtl_state{RTLState::IDLE}; /*< Current state in the state machine.*/ private: - /** - * @brief Get the horizontal wind velocity - * - * @return horizontal wind velocity. - */ - matrix::Vector2f get_wind(); - /** * @brief Set the return to launch control setpoint. * @@ -133,39 +127,10 @@ class RtlDirect : public MissionBlock, public ModuleParams void set_rtl_item(); /** - * @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 + * @brief sanitize land_approach * - * @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(); + loiter_point_s sanitizeLandApproach(loiter_point_s land_approach) const; /** * Check for parameter changes and update them if needed. @@ -178,8 +143,9 @@ class RtlDirect : public MissionBlock, public ModuleParams bool _enforce_rtl_alt{false}; bool _force_heading{false}; + RtlTimeEstimator _rtl_time_estimator; - DestinationPosition _destination; ///< the RTL position to fly to + PositionYawSetpoint _destination; ///< the RTL position to fly to loiter_point_s _land_approach; float _rtl_alt{0.0f}; ///< AMSL altitude at which the vehicle should return to the home position @@ -190,20 +156,10 @@ class RtlDirect : public MissionBlock, public ModuleParams (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}; + // external params + (ParamBool) _param_wv_en + ) uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::SubscriptionData _global_pos_sub{ORB_ID(vehicle_global_position)}; /**< global position subscription */ diff --git a/src/modules/navigator/rtl_direct_mission_land.cpp b/src/modules/navigator/rtl_direct_mission_land.cpp index 6712b0598dce..df6674e34ad8 100644 --- a/src/modules/navigator/rtl_direct_mission_land.cpp +++ b/src/modules/navigator/rtl_direct_mission_land.cpp @@ -52,6 +52,42 @@ RtlDirectMissionLand::RtlDirectMissionLand(Navigator *navigator) : } +void +RtlDirectMissionLand::updateDatamanCache() +{ + int32_t start_index; + + start_index = math::min(_mission.land_start_index, static_cast(_mission.count)); + + if ((start_index >= 0) && (_mission.count > 0) && hasMissionLandStart() && (start_index != _load_mission_index)) { + + int32_t end_index = static_cast(_mission.count); + + // Check that we load all data into the cache + if (end_index - start_index > _dataman_cache_size_signed) { + _dataman_cache.invalidate(); + _dataman_cache_size_signed = end_index - start_index; + _dataman_cache.resize(_dataman_cache_size_signed); + } + + for (int32_t index = start_index; index != end_index; index += math::signNoZero(_dataman_cache_size_signed)) { + + _dataman_cache.load(static_cast(_mission.mission_dataman_id), index); + } + + _load_mission_index = start_index; + } + + _dataman_cache.update(); +} + +void RtlDirectMissionLand::on_inactive() +{ + MissionBase::on_inactive(); + + updateDatamanCache(); +} + void RtlDirectMissionLand::on_activation() { _land_detected_sub.update(); @@ -62,13 +98,7 @@ void RtlDirectMissionLand::on_activation() 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; - - } + _needs_climbing = checkNeedsToClimb(); } else { _is_current_planned_mission_item_valid = false; @@ -146,11 +176,11 @@ void RtlDirectMissionLand::setActiveMissionItems() getNextPositionItems(_mission.current_seq + 1, next_mission_items_index, num_found_items, max_num_next_items); mission_item_s next_mission_items[max_num_next_items]; - const dm_item_t dataman_id = static_cast(_mission.dataman_id); + const dm_item_t mission_dataman_id = static_cast(_mission.mission_dataman_id); 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], + bool success = _dataman_cache.loadWait(mission_dataman_id, next_mission_items_index[i], reinterpret_cast(&next_mission_item), sizeof(next_mission_item), MAX_DATAMAN_LOAD_WAIT); if (success) { @@ -202,9 +232,171 @@ void RtlDirectMissionLand::setActiveMissionItems() 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(); + _rtl_time_estimator.update(); + _rtl_time_estimator.reset(); + + if (_mission.count > 0 && hasMissionLandStart()) { + int32_t start_item_index{-1}; + bool is_in_climbing_submode{false}; + + if (isActive()) { + start_item_index = math::max(_mission.current_seq, _mission.land_start_index); + is_in_climbing_submode = _needs_climbing; + + } else { + start_item_index = _mission.land_start_index; + is_in_climbing_submode = checkNeedsToClimb(); + } + + if (start_item_index >= 0 && start_item_index < static_cast(_mission.count)) { + float altitude_at_calculation_point; + matrix::Vector2d hor_position_at_calculation_point{_global_pos_sub.get().lat, _global_pos_sub.get().lon}; + + if (is_in_climbing_submode) { + if (_enforce_rtl_alt) { + _rtl_time_estimator.addVertDistance(_rtl_alt - _global_pos_sub.get().alt); + altitude_at_calculation_point = _rtl_alt; + + } else { + if (_global_pos_sub.get().alt < _rtl_alt) { + _rtl_time_estimator.addVertDistance(_rtl_alt - _global_pos_sub.get().alt); + } + + altitude_at_calculation_point = math::max(_rtl_alt, _global_pos_sub.get().alt); + } + + } else { + altitude_at_calculation_point = _global_pos_sub.get().alt; + } + + while (start_item_index < _mission.count && start_item_index >= 0) { + int32_t next_mission_item_index; + size_t num_found_items{0U}; + getNextPositionItems(start_item_index, &next_mission_item_index, num_found_items, 1U); + + if (num_found_items > 0U) { + mission_item_s next_position_mission_item; + const dm_item_t dataman_id = static_cast(_mission.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) { + // Could not load the mission item, mark time estimate as invalid. + _rtl_time_estimator.reset(); + break; + } + + switch (next_position_mission_item.nav_cmd) { + case NAV_CMD_LOITER_UNLIMITED: { + _rtl_time_estimator.reset(); + break; + } + + case NAV_CMD_LOITER_TIME_LIMIT: { + // Go to loiter + matrix::Vector2f direction{}; + get_vector_to_next_waypoint(hor_position_at_calculation_point(0), hor_position_at_calculation_point(1), + next_position_mission_item.lat, next_position_mission_item.lon, &direction(0), &direction(1)); + + float hor_dist = get_distance_to_next_waypoint(hor_position_at_calculation_point(0), + hor_position_at_calculation_point(1), next_position_mission_item.lat, next_position_mission_item.lon); + + _rtl_time_estimator.addDistance(hor_dist, direction, 0.f); + + // add time + _rtl_time_estimator.addWait(next_position_mission_item.time_inside); + break; + } + + case NAV_CMD_LOITER_TO_ALT: { + // Go to point horizontally + matrix::Vector2f direction{}; + get_vector_to_next_waypoint(hor_position_at_calculation_point(0), hor_position_at_calculation_point(1), + next_position_mission_item.lat, next_position_mission_item.lon, &direction(0), &direction(1)); + + float hor_dist = get_distance_to_next_waypoint(hor_position_at_calculation_point(0), + hor_position_at_calculation_point(1), next_position_mission_item.lat, next_position_mission_item.lon); + + _rtl_time_estimator.addDistance(hor_dist, direction, 0.f); + + // Add the vertical loiter + _rtl_time_estimator.addVertDistance(get_absolute_altitude_for_item(next_position_mission_item) - + altitude_at_calculation_point); + + break; + } + + case NAV_CMD_LAND: // Fallthrough + case NAV_CMD_VTOL_LAND: { + + matrix::Vector2f direction{}; + get_vector_to_next_waypoint(hor_position_at_calculation_point(0), hor_position_at_calculation_point(1), + next_position_mission_item.lat, next_position_mission_item.lon, &direction(0), &direction(1)); + + float hor_dist = get_distance_to_next_waypoint(hor_position_at_calculation_point(0), + hor_position_at_calculation_point(1), next_position_mission_item.lat, next_position_mission_item.lon); + + // For fixed wing, add diagonal line + if ((_vehicle_status_sub.get().vehicle_type != vehicle_status_s::VEHICLE_TYPE_FIXED_WING) + && (!_vehicle_status_sub.get().is_vtol)) { + + + _rtl_time_estimator.addDistance(hor_dist, direction, + get_absolute_altitude_for_item(next_position_mission_item) - altitude_at_calculation_point); + + } else { + // For VTOL, Rotary, go there horizontally first, then land + _rtl_time_estimator.addDistance(hor_dist, direction, 0.f); + + _rtl_time_estimator.addDescendMCLand(get_absolute_altitude_for_item(next_position_mission_item) - + altitude_at_calculation_point); + } + + break; + } + + default: { + // Default assume can go to the location directly + matrix::Vector2f direction{}; + get_vector_to_next_waypoint(hor_position_at_calculation_point(0), hor_position_at_calculation_point(1), + next_position_mission_item.lat, next_position_mission_item.lon, &direction(0), &direction(1)); + + float hor_dist = get_distance_to_next_waypoint(hor_position_at_calculation_point(0), + hor_position_at_calculation_point(1), next_position_mission_item.lat, next_position_mission_item.lon); + + _rtl_time_estimator.addDistance(hor_dist, direction, + get_absolute_altitude_for_item(next_position_mission_item) - altitude_at_calculation_point); + break; + } + } + + start_item_index = next_mission_item_index + 1; + hor_position_at_calculation_point(0) = next_position_mission_item.lat; + hor_position_at_calculation_point(1) = next_position_mission_item.lon; + altitude_at_calculation_point = get_absolute_altitude_for_item(next_position_mission_item); + + + } else { + start_item_index = -1; + } + } + } + } + + return _rtl_time_estimator.getEstimate(); +} + +bool RtlDirectMissionLand::checkNeedsToClimb() +{ + bool needs_climbing{false}; + + 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; + + } - return time_estimate; + return needs_climbing; } diff --git a/src/modules/navigator/rtl_direct_mission_land.h b/src/modules/navigator/rtl_direct_mission_land.h index 5edeafc71e5d..9bfdd620c4a4 100644 --- a/src/modules/navigator/rtl_direct_mission_land.h +++ b/src/modules/navigator/rtl_direct_mission_land.h @@ -43,6 +43,8 @@ #include "rtl_base.h" +#include + #include #include #include @@ -56,6 +58,7 @@ class RtlDirectMissionLand : public RtlBase ~RtlDirectMissionLand() = default; void on_activation() override; + void on_inactive() override; rtl_time_estimate_s calc_rtl_time_estimate() override; @@ -65,8 +68,12 @@ class RtlDirectMissionLand : public RtlBase private: bool setNextMissionItem() override; void setActiveMissionItems() override; + void updateDatamanCache() override; + bool checkNeedsToClimb(); 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 + + RtlTimeEstimator _rtl_time_estimator; }; diff --git a/src/modules/navigator/rtl_mission_fast.cpp b/src/modules/navigator/rtl_mission_fast.cpp index a84b1cddc9a6..70eb4819ffb0 100644 --- a/src/modules/navigator/rtl_mission_fast.cpp +++ b/src/modules/navigator/rtl_mission_fast.cpp @@ -97,11 +97,11 @@ void RtlMissionFast::setActiveMissionItems() getNextPositionItems(_mission.current_seq + 1, next_mission_items_index, num_found_items, max_num_next_items); mission_item_s next_mission_items[max_num_next_items]; - const dm_item_t dataman_id = static_cast(_mission.dataman_id); + const dm_item_t mission_dataman_id = static_cast(_mission.mission_dataman_id); 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], + bool success = _dataman_cache.loadWait(mission_dataman_id, next_mission_items_index[i], reinterpret_cast(&next_mission_item), sizeof(next_mission_item), MAX_DATAMAN_LOAD_WAIT); if (success) { diff --git a/src/modules/navigator/rtl_mission_fast_reverse.cpp b/src/modules/navigator/rtl_mission_fast_reverse.cpp index 92e6aef0a183..967e280bfd69 100644 --- a/src/modules/navigator/rtl_mission_fast_reverse.cpp +++ b/src/modules/navigator/rtl_mission_fast_reverse.cpp @@ -120,9 +120,9 @@ void RtlMissionFastReverse::setActiveMissionItems() if (num_found_items > 0) { - const dm_item_t dataman_id = static_cast(_mission.dataman_id); + const dm_item_t mission_dataman_id = static_cast(_mission.mission_dataman_id); mission_item_s next_mission_item; - bool success = _dataman_cache.loadWait(dataman_id, next_mission_item_index, + bool success = _dataman_cache.loadWait(mission_dataman_id, next_mission_item_index, reinterpret_cast(&next_mission_item), sizeof(mission_item_s), MAX_DATAMAN_LOAD_WAIT); if (success) { diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c index 7b4b573bc3e6..6fd734cc8e41 100644 --- a/src/modules/navigator/rtl_params.c +++ b/src/modules/navigator/rtl_params.c @@ -160,18 +160,6 @@ PARAM_DEFINE_INT32(RTL_PLD_MD, 0); */ PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 80.0f); -/** - * RTL heading mode - * - * Defines the heading behavior during RTL - * - * @value 0 Towards next waypoint. - * @value 1 Heading matches destination. - * @value 2 Use current heading. - * @group Return Mode - */ -PARAM_DEFINE_INT32(RTL_HDG_MD, 0); - /** * RTL time estimate safety margin factor * diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp index 05373e0e22d4..938e926a616f 100644 --- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp +++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp @@ -231,9 +231,12 @@ void VehicleAcceleration::Run() } // process all outstanding messages + int sensor_sub_updates = 0; sensor_accel_s sensor_data; - while (_sensor_sub.update(&sensor_data)) { + while ((sensor_sub_updates < sensor_accel_s::ORB_QUEUE_LENGTH) && _sensor_sub.update(&sensor_data)) { + sensor_sub_updates++; + const Vector3f accel_raw{sensor_data.x, sensor_data.y, sensor_data.z}; if (accel_raw.isAllFinite()) { diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp index d8654c4071ad..c18d670cfc44 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp @@ -163,9 +163,11 @@ void VehicleAirData::Run() } if (_advertised[uorb_index]) { + int sensor_sub_updates = 0; sensor_baro_s report; - while (_sensor_sub[uorb_index].update(&report)) { + while ((sensor_sub_updates < sensor_baro_s::ORB_QUEUE_LENGTH) && _sensor_sub[uorb_index].update(&report)) { + sensor_sub_updates++; if (_calibration[uorb_index].device_id() != report.device_id) { _calibration[uorb_index].set_device_id(report.device_id); diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index f2b0d3540b7d..b1eac451529a 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -822,9 +822,12 @@ void VehicleAngularVelocity::Run() if (_fifo_available) { // process all outstanding fifo messages + int sensor_sub_updates = 0; sensor_gyro_fifo_s sensor_fifo_data; - while (_sensor_gyro_fifo_sub.update(&sensor_fifo_data)) { + while ((sensor_sub_updates < sensor_gyro_fifo_s::ORB_QUEUE_LENGTH) && _sensor_gyro_fifo_sub.update(&sensor_fifo_data)) { + sensor_sub_updates++; + const float inverse_dt_s = 1e6f / sensor_fifo_data.dt; const int N = sensor_fifo_data.samples; static constexpr int FIFO_SIZE_MAX = sizeof(sensor_fifo_data.x) / sizeof(sensor_fifo_data.x[0]); @@ -863,9 +866,12 @@ void VehicleAngularVelocity::Run() } else { // process all outstanding messages + int sensor_sub_updates = 0; sensor_gyro_s sensor_data; - while (_sensor_sub.update(&sensor_data)) { + while ((sensor_sub_updates < sensor_gyro_s::ORB_QUEUE_LENGTH) && _sensor_sub.update(&sensor_data)) { + sensor_sub_updates++; + if (Vector3f(sensor_data.x, sensor_data.y, sensor_data.z).isAllFinite()) { if (_timestamp_sample_last == 0 || (sensor_data.timestamp_sample <= _timestamp_sample_last)) { diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp index 125d56758c69..f5b73434969d 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp @@ -194,7 +194,12 @@ void VehicleIMU::Run() // reset data gap monitor _data_gap = false; - while (_sensor_gyro_sub.updated() || _sensor_accel_sub.updated()) { + int sensor_sub_updates = 0; + + while ((_sensor_gyro_sub.updated() || _sensor_accel_sub.updated()) + && (sensor_sub_updates < math::max(sensor_accel_s::ORB_QUEUE_LENGTH, sensor_gyro_s::ORB_QUEUE_LENGTH))) { + sensor_sub_updates++; + bool updated = false; bool consume_all_gyro = !_intervals_configured || _data_gap; @@ -222,11 +227,16 @@ void VehicleIMU::Run() // update accel until integrator ready and caught up to gyro + int sensor_accel_sub_updates = 0; + while (_sensor_accel_sub.updated() + && (sensor_accel_sub_updates < sensor_accel_s::ORB_QUEUE_LENGTH) && (!_accel_integrator.integral_ready() || !_intervals_configured || _data_gap || (_accel_timestamp_sample_last < (_gyro_timestamp_sample_last - 0.5f * _accel_interval_us))) ) { + sensor_accel_sub_updates++; + if (UpdateAccel()) { updated = true; } @@ -647,6 +657,7 @@ bool VehicleIMU::Publish() imu.gyro_device_id = _gyro_calibration.device_id(); delta_angle_corrected.copyTo(imu.delta_angle); delta_velocity_corrected.copyTo(imu.delta_velocity); + imu.delta_angle_clipping = _delta_angle_clipping; imu.delta_velocity_clipping = _delta_velocity_clipping; imu.accel_calibration_count = _accel_calibration.calibration_count(); imu.gyro_calibration_count = _gyro_calibration.calibration_count(); @@ -654,6 +665,7 @@ bool VehicleIMU::Publish() _vehicle_imu_pub.publish(imu); // reset clip counts + _delta_angle_clipping = 0; _delta_velocity_clipping = 0; // record gyro publication latency and integrated samples diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp index 6bb7c5ef5b2c..df90ffdf9006 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp @@ -461,9 +461,11 @@ void VehicleMagnetometer::Run() } if (_advertised[uorb_index]) { + int sensor_mag_updates = 0; sensor_mag_s report; - while (_sensor_sub[uorb_index].update(&report)) { + while ((sensor_mag_updates < sensor_mag_s::ORB_QUEUE_LENGTH) && _sensor_sub[uorb_index].update(&report)) { + sensor_mag_updates++; if (_calibration[uorb_index].device_id() != report.device_id) { _calibration[uorb_index].set_device_id(report.device_id); diff --git a/src/modules/simulation/battery_simulator/battery_simulator_params.c b/src/modules/simulation/battery_simulator/battery_simulator_params.c index d634714cf3c9..986ae0b596bc 100644 --- a/src/modules/simulation/battery_simulator/battery_simulator_params.c +++ b/src/modules/simulation/battery_simulator/battery_simulator_params.c @@ -31,6 +31,18 @@ * ****************************************************************************/ +/** + * Simulator Battery enabled + * + * Enable or disable the internal battery simulation. This is useful + * when the battery is simulated externally and interfaced with PX4 + * through MAVLink for example. + * + * @boolean + * @group SITL + */ +PARAM_DEFINE_INT32(SIM_BAT_ENABLE, 1); + /** * Simulator Battery drain interval * diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index eda32df25cd3..2821213272c2 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -216,6 +216,15 @@ int GZBridge::init() return PX4_ERROR; } + // GPS: /world/$WORLD/model/$MODEL/link/base_link/sensor/navsat_sensor/navsat + std::string nav_sat_topic = "/world/" + _world_name + "/model/" + _model_name + + "/link/base_link/sensor/navsat_sensor/navsat"; + + if (!_node.Subscribe(nav_sat_topic, &GZBridge::navSatCallback, this)) { + PX4_ERR("failed to subscribe to %s", nav_sat_topic.c_str()); + return PX4_ERROR; + } + if (!_mixing_interface_esc.init(_model_name)) { PX4_ERR("failed to init ESC output"); return PX4_ERROR; @@ -565,10 +574,6 @@ void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &pose) vehicle_angular_velocity_groundtruth.timestamp = hrt_absolute_time(); _angular_velocity_ground_truth_pub.publish(vehicle_angular_velocity_groundtruth); - if (!_pos_ref.isInitialized()) { - _pos_ref.initReference((double)_param_sim_home_lat.get(), (double)_param_sim_home_lon.get(), hrt_absolute_time()); - } - vehicle_local_position_s local_position_groundtruth{}; #if defined(ENABLE_LOCKSTEP_SCHEDULER) local_position_groundtruth.timestamp_sample = time_us; @@ -595,31 +600,27 @@ void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &pose) local_position_groundtruth.heading = euler.psi(); - local_position_groundtruth.ref_lat = _pos_ref.getProjectionReferenceLat(); // Reference point latitude in degrees - local_position_groundtruth.ref_lon = _pos_ref.getProjectionReferenceLon(); // Reference point longitude in degrees - local_position_groundtruth.ref_alt = _param_sim_home_alt.get(); - local_position_groundtruth.ref_timestamp = _pos_ref.getProjectionReferenceTimestamp(); - - local_position_groundtruth.timestamp = hrt_absolute_time(); - _lpos_ground_truth_pub.publish(local_position_groundtruth); - if (_pos_ref.isInitialized()) { - // publish position groundtruth - vehicle_global_position_s global_position_groundtruth{}; -#if defined(ENABLE_LOCKSTEP_SCHEDULER) - global_position_groundtruth.timestamp_sample = time_us; -#else - global_position_groundtruth.timestamp_sample = hrt_absolute_time(); -#endif - _pos_ref.reproject(local_position_groundtruth.x, local_position_groundtruth.y, - global_position_groundtruth.lat, global_position_groundtruth.lon); + local_position_groundtruth.ref_lat = _pos_ref.getProjectionReferenceLat(); // Reference point latitude in degrees + local_position_groundtruth.ref_lon = _pos_ref.getProjectionReferenceLon(); // Reference point longitude in degrees + local_position_groundtruth.ref_alt = _alt_ref; + local_position_groundtruth.ref_timestamp = _pos_ref.getProjectionReferenceTimestamp(); + local_position_groundtruth.xy_global = true; + local_position_groundtruth.z_global = true; - global_position_groundtruth.alt = _param_sim_home_alt.get() - static_cast(position(2)); - global_position_groundtruth.timestamp = hrt_absolute_time(); - _gpos_ground_truth_pub.publish(global_position_groundtruth); + } else { + local_position_groundtruth.ref_lat = static_cast(NAN); + local_position_groundtruth.ref_lon = static_cast(NAN); + local_position_groundtruth.ref_alt = NAN; + local_position_groundtruth.ref_timestamp = 0; + local_position_groundtruth.xy_global = false; + local_position_groundtruth.z_global = false; } + local_position_groundtruth.timestamp = hrt_absolute_time(); + _lpos_ground_truth_pub.publish(local_position_groundtruth); + pthread_mutex_unlock(&_node_mutex); return; } @@ -704,6 +705,44 @@ void GZBridge::odometryCallback(const gz::msgs::OdometryWithCovariance &odometry pthread_mutex_unlock(&_node_mutex); } +void GZBridge::navSatCallback(const gz::msgs::NavSat &nav_sat) +{ + if (hrt_absolute_time() == 0) { + return; + } + + pthread_mutex_lock(&_node_mutex); + + const uint64_t time_us = (nav_sat.header().stamp().sec() * 1000000) + (nav_sat.header().stamp().nsec() / 1000); + + if (time_us > _world_time_us.load()) { + updateClock(nav_sat.header().stamp().sec(), nav_sat.header().stamp().nsec()); + } + + _timestamp_prev = time_us; + + // initialize gps position + if (!_pos_ref.isInitialized()) { + _pos_ref.initReference(nav_sat.latitude_deg(), nav_sat.longitude_deg(), hrt_absolute_time()); + _alt_ref = nav_sat.altitude(); + + } else { + // publish GPS groundtruth + vehicle_global_position_s global_position_groundtruth{}; +#if defined(ENABLE_LOCKSTEP_SCHEDULER) + global_position_groundtruth.timestamp_sample = time_us; +#else + global_position_groundtruth.timestamp_sample = hrt_absolute_time(); +#endif + global_position_groundtruth.lat = nav_sat.latitude_deg(); + global_position_groundtruth.lon = nav_sat.longitude_deg(); + global_position_groundtruth.alt = nav_sat.altitude(); + _gpos_ground_truth_pub.publish(global_position_groundtruth); + } + + pthread_mutex_unlock(&_node_mutex); +} + void GZBridge::rotateQuaternion(gz::math::Quaterniond &q_FRD_to_NED, const gz::math::Quaterniond q_FLU_to_ENU) { // FLU (ROS) to FRD (PX4) static rotation diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index 8bf7ab06c07f..6d2e2670a15e 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -105,6 +105,7 @@ class GZBridge : public ModuleBase, public ModuleParams, public px4::S void imuCallback(const gz::msgs::IMU &imu); void poseInfoCallback(const gz::msgs::Pose_V &pose); void odometryCallback(const gz::msgs::OdometryWithCovariance &odometry); + void navSatCallback(const gz::msgs::NavSat &nav_sat); /** * @@ -139,6 +140,7 @@ class GZBridge : public ModuleBase, public ModuleParams, public px4::S pthread_mutex_t _node_mutex; MapProjection _pos_ref{}; + double _alt_ref{}; // starting altitude reference matrix::Vector3d _position_prev{}; matrix::Vector3d _velocity_prev{}; @@ -153,10 +155,4 @@ class GZBridge : public ModuleBase, public ModuleParams, public px4::S float _temperature{288.15}; // 15 degrees gz::transport::Node _node; - - DEFINE_PARAMETERS( - (ParamFloat) _param_sim_home_lat, - (ParamFloat) _param_sim_home_lon, - (ParamFloat) _param_sim_home_alt - ) }; diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.cpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.cpp index a80286db6286..2fed14dfe716 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.cpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.cpp @@ -111,8 +111,7 @@ void GZMixingInterfaceWheel::wheelSpeedCallback(const gz::msgs::Actuators &actua wheel_encoders_s wheel_encoders{}; for (int i = 0; i < actuators.velocity_size(); i++) { - // Convert from RPM to rad/s - wheel_encoders.wheel_speed[i] = (float)actuators.velocity(i) * (2.0f * M_PI_F / 60.0f); + wheel_encoders.wheel_speed[i] = (float)actuators.velocity(i); } if (actuators.velocity_size() > 0) { diff --git a/src/modules/simulation/gz_bridge/parameters.c b/src/modules/simulation/gz_bridge/parameters.c index 513c67436cef..8dd6022ff6a0 100644 --- a/src/modules/simulation/gz_bridge/parameters.c +++ b/src/modules/simulation/gz_bridge/parameters.c @@ -40,26 +40,3 @@ */ PARAM_DEFINE_INT32(SIM_GZ_EN, 0); -/** - * simulator origin latitude - * - * @unit deg - * @group Simulator - */ -PARAM_DEFINE_FLOAT(SIM_GZ_HOME_LAT, 47.397742f); - -/** - * simulator origin longitude - * - * @unit deg - * @group Simulator - */ -PARAM_DEFINE_FLOAT(SIM_GZ_HOME_LON, 8.545594); - -/** - * simulator origin altitude - * - * @unit m - * @group Simulator - */ -PARAM_DEFINE_FLOAT(SIM_GZ_HOME_ALT, 488.0); diff --git a/src/modules/temperature_compensation/TemperatureCompensationModule.cpp b/src/modules/temperature_compensation/TemperatureCompensationModule.cpp index 1ed3283c3254..ec1935918164 100644 --- a/src/modules/temperature_compensation/TemperatureCompensationModule.cpp +++ b/src/modules/temperature_compensation/TemperatureCompensationModule.cpp @@ -106,7 +106,7 @@ void TemperatureCompensationModule::parameters_update() sensor_mag_s report; if (_mag_subs[uorb_index].copy(&report)) { - int temp = _temperature_compensation.set_sensor_id_accel(report.device_id, uorb_index); + int temp = _temperature_compensation.set_sensor_id_mag(report.device_id, uorb_index); if (temp < 0) { PX4_INFO("No temperature calibration available for mag %" PRIu8 " (device id %" PRIu32 ")", uorb_index, diff --git a/src/modules/temperature_compensation/temperature_calibration/mag.cpp b/src/modules/temperature_compensation/temperature_calibration/mag.cpp index 714e928af637..536b74b5f3a7 100644 --- a/src/modules/temperature_compensation/temperature_calibration/mag.cpp +++ b/src/modules/temperature_compensation/temperature_calibration/mag.cpp @@ -163,10 +163,10 @@ int TemperatureCalibrationMag::finish() } int32_t enabled = 1; - int result = param_set_no_notification(param_find("TC_A_ENABLE"), &enabled); + int result = param_set_no_notification(param_find("TC_M_ENABLE"), &enabled); if (result != PX4_OK) { - PX4_ERR("unable to reset TC_A_ENABLE (%i)", result); + PX4_ERR("unable to reset TC_M_ENABLE (%i)", result); } return result; diff --git a/src/modules/uuv_att_control/uuv_att_control.cpp b/src/modules/uuv_att_control/uuv_att_control.cpp index dd985c278873..58992ccb7803 100644 --- a/src/modules/uuv_att_control/uuv_att_control.cpp +++ b/src/modules/uuv_att_control/uuv_att_control.cpp @@ -138,13 +138,6 @@ void UUVAttitudeControl::control_attitude_geo(const vehicle_attitude_s &attitude */ Eulerf euler_angles(matrix::Quatf(attitude.q)); - float roll_u; - float pitch_u; - float yaw_u; - float thrust_x; - float thrust_y; - float thrust_z; - float roll_body = attitude_setpoint.roll_body; float pitch_body = attitude_setpoint.pitch_body; float yaw_body = attitude_setpoint.yaw_body; @@ -186,14 +179,14 @@ void UUVAttitudeControl::control_attitude_geo(const vehicle_attitude_s &attitude torques(1) = torques(1) - omega(1) * _param_pitch_d.get(); /**< Pitch */ torques(2) = torques(2) - omega(2) * _param_yaw_d.get(); /**< Yaw */ - roll_u = torques(0); - pitch_u = torques(1); - yaw_u = torques(2); + float roll_u = torques(0); + float pitch_u = torques(1); + float yaw_u = torques(2); // take thrust as - thrust_x = attitude_setpoint.thrust_body[0]; - thrust_y = attitude_setpoint.thrust_body[1]; - thrust_z = attitude_setpoint.thrust_body[2]; + float thrust_x = attitude_setpoint.thrust_body[0]; + float thrust_y = attitude_setpoint.thrust_body[1]; + float thrust_z = attitude_setpoint.thrust_body[2]; constrain_actuator_commands(roll_u, pitch_u, yaw_u, thrust_x, thrust_y, thrust_z); diff --git a/src/modules/uxrce_dds_client/dds_topics.h.em b/src/modules/uxrce_dds_client/dds_topics.h.em index c664d87a9f7d..cade47b5fdce 100644 --- a/src/modules/uxrce_dds_client/dds_topics.h.em +++ b/src/modules/uxrce_dds_client/dds_topics.h.em @@ -168,7 +168,7 @@ bool RcvTopicsPubs::init(uxrSession *session, uxrStreamId reliable_out_stream_id { @[ for idx, sub in enumerate(subscriptions + subscriptions_multi)]@ { - uint16_t queue_depth = uORB::DefaultQueueSize<@(sub['simple_base_type'])_s>::value * 2; // use a bit larger queue size than internal + uint16_t queue_depth = orb_get_queue_size(ORB_ID(@(sub['simple_base_type']))) * 2; // use a bit larger queue size than internal create_data_reader(session, reliable_out_stream_id, best_effort_in_stream_id, participant_id, @(idx), client_namespace, "@(sub['topic_simple'])", "@(sub['dds_type'])", queue_depth); } @[ end for]@ diff --git a/src/modules/uxrce_dds_client/uxrce_dds_client.cpp b/src/modules/uxrce_dds_client/uxrce_dds_client.cpp index 43a391de1131..1412414b6300 100644 --- a/src/modules/uxrce_dds_client/uxrce_dds_client.cpp +++ b/src/modules/uxrce_dds_client/uxrce_dds_client.cpp @@ -49,10 +49,6 @@ #include #include -#if defined(CONFIG_NET) || defined(__PX4_POSIX) -# define UXRCE_DDS_CLIENT_UDP 1 -#endif - #define STREAM_HISTORY 4 #define BUFFER_SIZE (UXR_CONFIG_SERIAL_TRANSPORT_MTU * STREAM_HISTORY) // MTU==512 by default @@ -60,8 +56,8 @@ using namespace time_literals; -void on_time(uxrSession *session, int64_t current_time, int64_t received_timestamp, int64_t transmit_timestamp, - int64_t originate_timestamp, void *args) +static void on_time(uxrSession *session, int64_t current_time, int64_t received_timestamp, int64_t transmit_timestamp, + int64_t originate_timestamp, void *args) { // latest round trip time (RTT) int64_t rtt = current_time - originate_timestamp; @@ -80,21 +76,15 @@ 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) +static 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; } - -void on_request( - uxrSession *session, - uxrObjectId object_id, - uint16_t request_id, - SampleIdentity *sample_id, - ucdrBuffer *ub, - uint16_t length, - void *args) +static void on_request(uxrSession *session, uxrObjectId object_id, uint16_t request_id, SampleIdentity *sample_id, + ucdrBuffer *ub, uint16_t length, void *args) { (void) request_id; (void) length; @@ -110,66 +100,115 @@ void on_request( UxrceddsClient::UxrceddsClient(Transport transport, const char *device, int baudrate, const char *agent_ip, const char *port, const char *client_namespace) : ModuleParams(nullptr), + _transport(transport), + _baudrate(baudrate), _client_namespace(client_namespace) { - if (transport == Transport::Serial) { + if (device) { + // store serial port name */ + strncpy(_device, device, sizeof(_device) - 1); + } - int fd = -1; +#if defined(UXRCE_DDS_CLIENT_UDP) - for (int attempt = 0; attempt < 3; attempt++) { - fd = open(device, O_RDWR | O_NOCTTY | O_NONBLOCK); + if (agent_ip) { + strncpy(_agent_ip, agent_ip, sizeof(_agent_ip) - 1); + } - if (fd < 0) { - PX4_ERR("open %s failed (%i)", device, errno); - // sleep before trying again - px4_usleep(1_s); + if (port) { + strncpy(_port, port, sizeof(_port) - 1); + } - } else { - break; - } +#endif // UXRCE_DDS_CLIENT_UDP +} + +bool UxrceddsClient::init() +{ + deinit(); + + if (_transport == Transport::Serial) { + int fd = open(_device, O_RDWR | O_NOCTTY | O_NONBLOCK); + + if (fd < 0) { + PX4_ERR("open %s failed (%i)", _device, errno); + return false; } _transport_serial = new uxrSerialTransport(); - if (fd >= 0 && setBaudrate(fd, baudrate) == 0 && _transport_serial) { - // TODO: - uint8_t remote_addr = 0; // Identifier of the Agent in the connection - uint8_t local_addr = 1; // Identifier of the Client in the serial connection + // TODO: + uint8_t remote_addr = 0; // Identifier of the Agent in the connection + uint8_t local_addr = 1; // Identifier of the Client in the serial connection - if (uxr_init_serial_transport(_transport_serial, fd, remote_addr, local_addr)) { - _comm = &_transport_serial->comm; - _fd = fd; + if (_transport_serial + && setBaudrate(fd, _baudrate) + && uxr_init_serial_transport(_transport_serial, fd, remote_addr, local_addr) + ) { + PX4_INFO("init serial %s @ %d baud", _device, _baudrate); - } else { - PX4_ERR("uxr_init_serial_transport failed"); - } + _comm = &_transport_serial->comm; + _fd = fd; + + return true; } - } else if (transport == Transport::Udp) { + PX4_ERR("init serial %s @ %d baud failed", _device, _baudrate); + close(fd); + + delete _transport_serial; + _transport_serial = nullptr; + + return false; + } #if defined(UXRCE_DDS_CLIENT_UDP) + + if (_transport == Transport::Udp) { _transport_udp = new uxrUDPTransport(); - strncpy(_port, port, PORT_MAX_LENGTH - 1); - strncpy(_agent_ip, agent_ip, AGENT_IP_MAX_LENGTH - 1); - if (_transport_udp) { - if (uxr_init_udp_transport(_transport_udp, UXR_IPv4, _agent_ip, _port)) { - _comm = &_transport_udp->comm; - _fd = _transport_udp->platform.poll_fd.fd; + if (_transport_udp && uxr_init_udp_transport(_transport_udp, UXR_IPv4, _agent_ip, _port)) { - } else { - PX4_ERR("uxr_init_udp_transport failed"); - } + PX4_INFO("init UDP agent IP:%s, port:%s", _agent_ip, _port); + + _comm = &_transport_udp->comm; + _fd = _transport_udp->platform.poll_fd.fd; + + return true; + + } else { + PX4_ERR("init UDP agent IP:%s, port:%s failed", _agent_ip, _port); } + } +#endif // UXRCE_DDS_CLIENT_UDP -#else - PX4_ERR("UDP not supported"); -#endif + return false; +} + +void UxrceddsClient::deinit() +{ + if (_fd >= 0) { + close(_fd); + _fd = -1; + } + + if (_transport_serial) { + uxr_close_serial_transport(_transport_serial); + delete _transport_serial; + _transport_serial = nullptr; } - _participant_config = static_cast(_param_uxrce_dds_ptcfg.get()); - _synchronize_timestamps = _param_uxrce_dds_synct.get() > 0; +#if defined(UXRCE_DDS_CLIENT_UDP) + + if (_transport_udp) { + uxr_close_udp_transport(_transport_udp); + delete _transport_udp; + _transport_udp = nullptr; + } + +#endif // UXRCE_DDS_CLIENT_UDP + + _comm = nullptr; } UxrceddsClient::~UxrceddsClient() @@ -184,10 +223,17 @@ UxrceddsClient::~UxrceddsClient() delete _transport_serial; } + perf_free(_loop_perf); + perf_free(_loop_interval_perf); + +#if defined(UXRCE_DDS_CLIENT_UDP) + if (_transport_udp) { uxr_close_udp_transport(_transport_udp); delete _transport_udp; } + +#endif // UXRCE_DDS_CLIENT_UDP } static void fillMessageFormatResponse(const message_format_request_s &message_format_request, @@ -286,11 +332,6 @@ void UxrceddsClient::syncSystemClock(uxrSession *session) void UxrceddsClient::run() { - if (!_comm) { - PX4_ERR("init failed"); - return; - } - _subs = new SendTopicsSubs(); _pubs = new RcvTopicsPubs(); @@ -300,6 +341,17 @@ void UxrceddsClient::run() } while (!should_exit()) { + + while (!should_exit() && !_comm) { + if (!init()) { + // sleep before trying again + px4_usleep(1'000'000); + } + } + + _participant_config = static_cast(_param_uxrce_dds_ptcfg.get()); + _synchronize_timestamps = (_param_uxrce_dds_synct.get() > 0); + bool got_response = false; while (!should_exit() && !got_response) { @@ -419,9 +471,9 @@ void UxrceddsClient::run() } // create VehicleCommand replier - if (num_of_repliers < MAX_NUM_REPLIERS) { + if (_num_of_repliers < MAX_NUM_REPLIERS) { if (add_replier(new VehicleCommandSrv(&session, reliable_out, reliable_in, participant_id, _client_namespace, - num_of_repliers))) { + _num_of_repliers))) { PX4_ERR("replier init failed"); return; } @@ -451,7 +503,7 @@ void UxrceddsClient::run() break; } - px4_usleep(10_ms); + px4_usleep(10'000); } hrt_abstime last_sync_session = 0; @@ -467,45 +519,48 @@ void UxrceddsClient::run() 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); + perf_begin(_loop_perf); + perf_count(_loop_interval_perf); - /* 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); - } + int orb_poll_timeout_ms = 10; + + int bytes_available = 0; - poll_error_counter++; - continue; + if (ioctl(_fd, FIONREAD, (unsigned long)&bytes_available) == OK) { + if (bytes_available > 10) { + orb_poll_timeout_ms = 0; + } } - _subs->update(&session, reliable_out, best_effort_out, participant_id, _client_namespace); + /* Wait for topic updates for max 10 ms */ + int poll = px4_poll(_subs->fds, (sizeof(_subs->fds) / sizeof(_subs->fds[0])), orb_poll_timeout_ms); - // check if there are available replies - process_replies(); + /* Handle the poll results */ + if (poll > 0) { + _subs->update(&session, reliable_out, best_effort_out, participant_id, _client_namespace); - // Run the session until we receive no more data or up to a maximum number of iterations. - // The maximum observed number of iterations was 6 (SITL). If we were to run only once, data starts to get - // delayed, causing registered flight modes to time out. - for (int i = 0; i < 10; ++i) { - const uint32_t prev_num_payload_received = _pubs->num_payload_received; - uxr_run_session_timeout(&session, 0); + } else { + if (poll < 0) { + // poll error + if (poll_error_counter < 10 || poll_error_counter % 50 == 0) { + // prevent flooding + PX4_ERR("ERROR while polling uorbs: %d", poll); + } - if (_pubs->num_payload_received == prev_num_payload_received) { - break; + poll_error_counter++; } } + // run session with 0 timeout (non-blocking) + uxr_run_session_timeout(&session, 0); + + // check if there are available replies + process_replies(); + // time sync session if (_synchronize_timestamps && hrt_elapsed_time(&last_sync_session) > 1_s) { - if (uxr_sync_session(&session, 100) && _timesync.sync_converged()) { + + if (uxr_sync_session(&session, 10) && _timesync.sync_converged()) { //PX4_INFO("synchronized with time offset %-5" PRId64 "ns", session.time_offset); last_sync_session = hrt_absolute_time(); @@ -513,6 +568,15 @@ void UxrceddsClient::run() syncSystemClock(&session); } } + + if (!_timesync_converged && _timesync.sync_converged()) { + PX4_INFO("time sync converged"); + + } else if (_timesync_converged && !_timesync.sync_converged()) { + PX4_WARN("time sync no longer converged"); + } + + _timesync_converged = _timesync.sync_converged(); } handleMessageFormatRequest(); @@ -534,27 +598,38 @@ void UxrceddsClient::run() last_status_update = now; } - // Handle ping - if (now - last_ping > 500_ms) { + // Handle ping, unless we're actively sending & receiving payloads successfully + if ((_last_payload_tx_rate > 0) && (_last_payload_rx_rate > 0)) { + _connected = true; + num_pings_missed = 0; last_ping = now; - if (had_ping_reply) { - num_pings_missed = 0; + } else { + if (hrt_elapsed_time(&last_ping) > 1_s) { + last_ping = now; - } else { - ++num_pings_missed; - } + if (had_ping_reply) { + num_pings_missed = 0; + + } else { + ++num_pings_missed; + } - uxr_ping_agent_session(&session, 0, 1); + int timeout_ms = 1'000; // 1 second + uint8_t attempts = 1; + uxr_ping_agent_session(&session, timeout_ms, attempts); - had_ping_reply = false; - } + had_ping_reply = false; + } - if (num_pings_missed > 2) { - PX4_INFO("No ping response, disconnecting"); - _connected = false; + if (num_pings_missed >= 3) { + PX4_INFO("No ping response, disconnecting"); + _connected = false; + } } + perf_end(_loop_perf); + } delete_repliers(); @@ -567,7 +642,7 @@ void UxrceddsClient::run() } } -int UxrceddsClient::setBaudrate(int fd, unsigned baud) +bool UxrceddsClient::setBaudrate(int fd, unsigned baud) { int speed; @@ -640,7 +715,7 @@ int UxrceddsClient::setBaudrate(int fd, unsigned baud) default: PX4_ERR("ERR: unknown baudrate: %d", baud); - return -EINVAL; + return false; } struct termios uart_config; @@ -688,28 +763,28 @@ int UxrceddsClient::setBaudrate(int fd, unsigned baud) /* set baud rate */ if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { PX4_ERR("ERR: %d (cfsetispeed)", termios_state); - return -1; + return false; } if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { PX4_ERR("ERR: %d (cfsetospeed)", termios_state); - return -1; + return false; } if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) { PX4_ERR("ERR: %d (tcsetattr)", termios_state); - return -1; + return false; } - return 0; + return true; } bool UxrceddsClient::add_replier(SrvBase *replier) { - if (num_of_repliers < MAX_NUM_REPLIERS) { - repliers_[num_of_repliers] = replier; + if (_num_of_repliers < MAX_NUM_REPLIERS) { + _repliers[_num_of_repliers] = replier; - num_of_repliers++; + _num_of_repliers++; } return false; @@ -718,11 +793,12 @@ bool UxrceddsClient::add_replier(SrvBase *replier) void UxrceddsClient::process_requests(uxrObjectId object_id, SampleIdentity *sample_id, ucdrBuffer *ub, const int64_t time_offset_us) { - for (uint8_t i = 0; i < num_of_repliers; i++) { - if (object_id.id == repliers_[i]->replier_id_.id - && object_id.type == repliers_[i]->replier_id_.type) { - repliers_[i]->process_request(ub, time_offset_us); - memcpy(&(repliers_[i]->sample_id_), sample_id, sizeof(repliers_[i]->sample_id_)); + for (uint8_t i = 0; i < _num_of_repliers; i++) { + if (object_id.id == _repliers[i]->replier_id_.id + && object_id.type == _repliers[i]->replier_id_.type) { + + _repliers[i]->process_request(ub, time_offset_us); + memcpy(&(_repliers[i]->sample_id_), sample_id, sizeof(_repliers[i]->sample_id_)); break; } } @@ -730,18 +806,19 @@ void UxrceddsClient::process_requests(uxrObjectId object_id, SampleIdentity *sam void UxrceddsClient::process_replies() { - for (uint8_t i = 0; i < num_of_repliers; i++) { - repliers_[i]->process_reply(); + for (uint8_t i = 0; i < _num_of_repliers; i++) { + _repliers[i]->process_reply(); } } void UxrceddsClient::delete_repliers() { - for (uint8_t i = 0; i < num_of_repliers; i++) { - delete (repliers_[i]); + for (uint8_t i = 0; i < _num_of_repliers; i++) { + delete (_repliers[i]); + _repliers[i] = nullptr; } - num_of_repliers = 0; + _num_of_repliers = 0; } int UxrceddsClient::custom_command(int argc, char *argv[]) @@ -790,6 +867,11 @@ int UxrceddsClient::print_status() PX4_INFO("Payload rx: %i B/s", _last_payload_rx_rate); } + PX4_INFO("timesync converged: %s", _timesync.sync_converged() ? "true" : "false"); + + perf_print_counter(_loop_perf); + perf_print_counter(_loop_interval_perf); + return 0; } diff --git a/src/modules/uxrce_dds_client/uxrce_dds_client.h b/src/modules/uxrce_dds_client/uxrce_dds_client.h index b8f38ee76c66..a0c563b27963 100644 --- a/src/modules/uxrce_dds_client/uxrce_dds_client.h +++ b/src/modules/uxrce_dds_client/uxrce_dds_client.h @@ -44,6 +44,12 @@ #include +#include + +#if defined(CONFIG_NET) || defined(__PX4_POSIX) +# define UXRCE_DDS_CLIENT_UDP 1 +#endif + #include "srv_base.h" #define MAX_NUM_REPLIERS 5 @@ -108,7 +114,11 @@ class UxrceddsClient : public ModuleBase, public ModuleParams void delete_repliers(); private: - int setBaudrate(int fd, unsigned baud); + + bool init(); + void deinit(); + + bool setBaudrate(int fd, unsigned baud); void handleMessageFormatRequest(); @@ -118,6 +128,12 @@ class UxrceddsClient : public ModuleBase, public ModuleParams /** Synchronizes the system clock if the time is off by more than 5 seconds */ void syncSystemClock(uxrSession *session); + Transport _transport{}; + + uxrSerialTransport *_transport_serial{nullptr}; + char _device[32] {}; + int _baudrate{}; + const char *_client_namespace; enum class ParticipantConfig { @@ -130,22 +146,22 @@ class UxrceddsClient : public ModuleBase, public ModuleParams // max port characters (5+'\0') static const uint8_t PORT_MAX_LENGTH = 6; + // max agent ip characters (15+'\0') static const uint8_t AGENT_IP_MAX_LENGTH = 16; -#if defined(CONFIG_NET) || defined(__PX4_POSIX) - char _port[PORT_MAX_LENGTH]; - char _agent_ip[AGENT_IP_MAX_LENGTH]; -#endif +#if defined(UXRCE_DDS_CLIENT_UDP) + char _port[PORT_MAX_LENGTH] {}; + char _agent_ip[AGENT_IP_MAX_LENGTH] {}; + uxrUDPTransport *_transport_udp{nullptr}; +#endif // UXRCE_DDS_CLIENT_UDP SendTopicsSubs *_subs{nullptr}; RcvTopicsPubs *_pubs{nullptr}; - SrvBase *repliers_[MAX_NUM_REPLIERS]; - uint8_t num_of_repliers{0}; + SrvBase *_repliers[MAX_NUM_REPLIERS]; + uint8_t _num_of_repliers{0}; - uxrSerialTransport *_transport_serial{nullptr}; - uxrUDPTransport *_transport_udp{nullptr}; uxrCommunication *_comm{nullptr}; int _fd{-1}; @@ -153,8 +169,13 @@ class UxrceddsClient : public ModuleBase, public ModuleParams int _last_payload_rx_rate{}; ///< in B/s bool _connected{false}; + bool _timesync_converged{false}; + Timesync _timesync{timesync_status_s::SOURCE_PROTOCOL_DDS}; + perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")}; + DEFINE_PARAMETERS( (ParamInt) _param_uxrce_dds_dom_id, (ParamInt) _param_uxrce_key, diff --git a/src/modules/uxrce_dds_client/vehicle_command_srv.cpp b/src/modules/uxrce_dds_client/vehicle_command_srv.cpp index aac0bd2c5857..c25d6757d4a9 100644 --- a/src/modules/uxrce_dds_client/vehicle_command_srv.cpp +++ b/src/modules/uxrce_dds_client/vehicle_command_srv.cpp @@ -39,8 +39,7 @@ VehicleCommandSrv::VehicleCommandSrv(uxrSession *session, uxrStreamId reliable_o uxrStreamId input_stream_id, uxrObjectId participant_id, const char *client_namespace, const uint8_t index) : SrvBase(session, reliable_out_stream_id, input_stream_id, participant_id) { - uint16_t queue_depth = uORB::DefaultQueueSize::value * - 2; // use a bit larger queue size than internal + uint16_t queue_depth = orb_get_queue_size(ORB_ID(vehicle_command)) * 2; // use a bit larger queue size than internal create_replier(input_stream_id, participant_id, index, client_namespace, "vehicle_command", "VehicleCommand", queue_depth); }; diff --git a/src/modules/vtol_att_control/standard_params.c b/src/modules/vtol_att_control/standard_params.c index 51b75a159b06..f46f644f181d 100644 --- a/src/modules/vtol_att_control/standard_params.c +++ b/src/modules/vtol_att_control/standard_params.c @@ -48,6 +48,7 @@ * Fixed-wing forward actuators refers to puller/pusher (standard VTOL), or forward-tilt (tiltrotor VTOL). * Only active if demanded down pitch is below VT_PITCH_MIN. * Use VT_FWD_THRUST_SC to tune it. + * Descend mode is treated as Landing too. * * Only active (if enabled) in Altitude, Position and Auto modes, not in Stabilized. * diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 269f9645a4d6..b16a344ca6e8 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -88,15 +88,8 @@ void Tailsitter::update_vtol_state() case vtol_mode::TRANSITION_BACK: const float pitch = Eulerf(Quatf(_v_att->q)).theta(); - float pitch_threshold_mc = PITCH_THRESHOLD_AUTO_TRANSITION_TO_MC; - - // if doing transition in Stabilized mode set threshold to max angle plus 5° margin - if (!_v_control_mode->flag_control_altitude_enabled) { - pitch_threshold_mc = math::radians(-_param_mpc_tilt_max.get() - 5.f); - } - // check if we have reached pitch angle to switch to MC mode - if (pitch >= pitch_threshold_mc || _time_since_trans_start > _param_vt_b_trans_dur.get()) { + if (pitch >= PITCH_THRESHOLD_AUTO_TRANSITION_TO_MC || _time_since_trans_start > _param_vt_b_trans_dur.get()) { _vtol_mode = vtol_mode::MC_MODE; } @@ -172,31 +165,30 @@ void Tailsitter::update_transition_state() // calculate rotation axis for transition. _q_trans_start = Quatf(_v_att->q); Vector3f z = -_q_trans_start.dcm_z(); - _trans_rot_axis = z.cross(Vector3f(0, 0, -1)); + _trans_rot_axis = z.cross(Vector3f(0.f, 0.f, -1.f)); // as heading setpoint we choose the heading given by the direction the vehicle points - float yaw_sp = atan2f(z(1), z(0)); + const float yaw_sp = atan2f(z(1), z(0)); // the intial attitude setpoint for a backtransition is a combination of the current fw pitch setpoint, // the yaw setpoint and zero roll since we want wings level transition. - // If for some reason the fw attitude setpoint is not recent then don't sue it and assume 0 pitch + // If for some reason the fw attitude setpoint is not recent then don't use it and assume 0 pitch if (_fw_virtual_att_sp->timestamp > (now - 1_s)) { - _q_trans_start = Eulerf(0.0f, _fw_virtual_att_sp->pitch_body, yaw_sp); + _q_trans_start = Eulerf(0.f, _fw_virtual_att_sp->pitch_body, yaw_sp); } else { - _q_trans_start = Eulerf(0.0f, 0.f, yaw_sp); + _q_trans_start = Eulerf(0.f, 0.f, yaw_sp); } - // attitude during transitions are controlled by mc attitude control so rotate the desired attitude to the // multirotor frame _q_trans_start = _q_trans_start * Quatf(Eulerf(0, -M_PI_2_F, 0)); } else if (_vtol_mode == vtol_mode::TRANSITION_FRONT_P1) { // initial attitude setpoint for the transition should be with wings level - _q_trans_start = Eulerf(0.0f, _mc_virtual_att_sp->pitch_body, _mc_virtual_att_sp->yaw_body); - Vector3f x = Dcmf(Quatf(_v_att->q)) * Vector3f(1, 0, 0); - _trans_rot_axis = -x.cross(Vector3f(0, 0, -1)); + _q_trans_start = Eulerf(0.f, _mc_virtual_att_sp->pitch_body, _mc_virtual_att_sp->yaw_body); + Vector3f x = Dcmf(Quatf(_v_att->q)) * Vector3f(1.f, 0.f, 0.f); + _trans_rot_axis = -x.cross(Vector3f(0.f, 0.f, -1.f)); } _q_trans_sp = _q_trans_start; @@ -206,10 +198,8 @@ void Tailsitter::update_transition_state() _q_trans_sp.normalize(); // tilt angle (zero if vehicle nose points up (hover)) - float cos_tilt = _q_trans_sp(0) * _q_trans_sp(0) - _q_trans_sp(1) * _q_trans_sp(1) - _q_trans_sp(2) * - _q_trans_sp(2) + _q_trans_sp(3) * _q_trans_sp(3); - cos_tilt = cos_tilt > 1.0f ? 1.0f : cos_tilt; - cos_tilt = cos_tilt < -1.0f ? -1.0f : cos_tilt; + const float cos_tilt = math::constrain(_q_trans_sp(0) * _q_trans_sp(0) - _q_trans_sp(1) * _q_trans_sp(1) - + _q_trans_sp(2) * _q_trans_sp(2) + _q_trans_sp(3) * _q_trans_sp(3), -1.f, 1.f); const float tilt = acosf(cos_tilt); if (_vtol_mode == vtol_mode::TRANSITION_FRONT_P1) { @@ -329,14 +319,7 @@ bool Tailsitter::isFrontTransitionCompletedBase() bool transition_to_fw = false; const float pitch = Eulerf(Quatf(_v_att->q)).theta(); - float pitch_threshold_fw = PITCH_THRESHOLD_AUTO_TRANSITION_TO_FW; - - // if doing transition in Stabilized mode set threshold to max angle minus 5° margin - if (!_v_control_mode->flag_control_altitude_enabled) { - pitch_threshold_fw = math::radians(-_param_mpc_tilt_max.get() + 5.f); - } - - if (pitch <= pitch_threshold_fw) { + if (pitch <= PITCH_THRESHOLD_AUTO_TRANSITION_TO_FW) { if (airspeed_triggers_transition) { transition_to_fw = _airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_trans.get() ; diff --git a/src/modules/vtol_att_control/tailsitter.h b/src/modules/vtol_att_control/tailsitter.h index 3770caec2c1f..93fa0b2491be 100644 --- a/src/modules/vtol_att_control/tailsitter.h +++ b/src/modules/vtol_att_control/tailsitter.h @@ -88,8 +88,7 @@ class Tailsitter : public VtolType bool isFrontTransitionCompletedBase() override; DEFINE_PARAMETERS_CUSTOM_PARENT(VtolType, - (ParamFloat) _param_fw_psp_off, - (ParamFloat) _param_mpc_tilt_max + (ParamFloat) _param_fw_psp_off ) diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 6e023da689e3..0956e1003698 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -245,10 +245,8 @@ void Tiltrotor::update_transition_state() if (_param_fw_use_airspd.get() && PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) && _airspeed_validated->calibrated_airspeed_m_s >= getBlendAirspeed()) { - const float weight = 1.0f - (_airspeed_validated->calibrated_airspeed_m_s - getBlendAirspeed()) / - (getTransitionAirspeed() - getBlendAirspeed()); - _mc_roll_weight = weight; - _mc_yaw_weight = weight; + _mc_roll_weight = 1.0f - (_airspeed_validated->calibrated_airspeed_m_s - getBlendAirspeed()) / + (getTransitionAirspeed() - getBlendAirspeed()); } // without airspeed do timed weight changes @@ -256,7 +254,6 @@ void Tiltrotor::update_transition_state() _time_since_trans_start > getMinimumFrontTransitionTime()) { _mc_roll_weight = 1.0f - (_time_since_trans_start - getMinimumFrontTransitionTime()) / (getOpenLoopFrontTransitionTime() - getMinimumFrontTransitionTime()); - _mc_yaw_weight = _mc_roll_weight; } // add minimum throttle for front transition diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index eccdf783058c..b464793c99ce 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -206,45 +206,41 @@ void VtolAttitudeControl::quadchute(QuadchuteReason reason) { if (!_vtol_vehicle_status.fixed_wing_system_failure) { + // only publish generic warning through mavlink to safe flash + mavlink_log_critical(&_mavlink_log_pub, "Quad-chute triggered\t"); + switch (reason) { case QuadchuteReason::TransitionTimeout: - mavlink_log_critical(&_mavlink_log_pub, "Quadchute: transition timeout\t"); events::send(events::ID("vtol_att_ctrl_quadchute_tout"), events::Log::Critical, "Quad-chute triggered due to transition timeout"); break; case QuadchuteReason::ExternalCommand: - mavlink_log_critical(&_mavlink_log_pub, "Quadchute: external command\t"); events::send(events::ID("vtol_att_ctrl_quadchute_ext_cmd"), events::Log::Critical, "Quad-chute triggered due to external command"); break; case QuadchuteReason::MinimumAltBreached: - mavlink_log_critical(&_mavlink_log_pub, "Quadchute: minimum altitude breached\t"); events::send(events::ID("vtol_att_ctrl_quadchute_min_alt"), events::Log::Critical, "Quad-chute triggered due to minimum altitude breach"); break; case QuadchuteReason::UncommandedDescent: - mavlink_log_critical(&_mavlink_log_pub, "Quadchute: Uncommanded descent detected\t"); events::send(events::ID("vtol_att_ctrl_quadchute_alt_loss"), events::Log::Critical, "Quad-chute triggered due to uncommanded descent detection"); break; case QuadchuteReason::TransitionAltitudeLoss: - mavlink_log_critical(&_mavlink_log_pub, "Quadchute: loss of altitude during transition\t"); events::send(events::ID("vtol_att_ctrl_quadchute_trans_alt_err"), events::Log::Critical, "Quad-chute triggered due to loss of altitude during transition"); break; case QuadchuteReason::MaximumPitchExceeded: - mavlink_log_critical(&_mavlink_log_pub, "Quadchute: maximum pitch exceeded\t"); events::send(events::ID("vtol_att_ctrl_quadchute_max_pitch"), events::Log::Critical, "Quad-chute triggered due to maximum pitch angle exceeded"); break; case QuadchuteReason::MaximumRollExceeded: - mavlink_log_critical(&_mavlink_log_pub, "Quadchute: maximum roll exceeded\t"); events::send(events::ID("vtol_att_ctrl_quadchute_max_roll"), events::Log::Critical, "Quad-chute triggered due to maximum roll angle exceeded"); break; diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 2695da5cf21e..808c9c03c2a9 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -286,13 +286,17 @@ bool VtolType::isUncommandedDescent() && (current_altitude < _tecs_status->altitude_reference) && hrt_elapsed_time(&_tecs_status->timestamp) < 1_s) { + if (!PX4_ISFINITE(_quadchute_ref_alt)) { + _quadchute_ref_alt = current_altitude; + } + _quadchute_ref_alt = math::min(math::max(_quadchute_ref_alt, current_altitude), _tecs_status->altitude_reference); return (_quadchute_ref_alt - current_altitude) > _param_vt_qc_alt_loss.get(); } else { - _quadchute_ref_alt = -MAXFLOAT; + _quadchute_ref_alt = NAN; } return false; @@ -318,6 +322,11 @@ void VtolType::handleEkfResets() if (_local_pos->z_reset_counter != _altitude_reset_counter) { _local_position_z_start_of_transition += _local_pos->delta_z; _altitude_reset_counter = _local_pos->z_reset_counter; + + if (PX4_ISFINITE(_quadchute_ref_alt)) { + _quadchute_ref_alt += _local_pos->delta_z; + } + } } @@ -435,16 +444,21 @@ float VtolType::pusher_assist() } + // the vehicle is "landing" if it is in auto mode and the type is set to LAND, and + // "descending" if it is in auto and climb rate controlled but not altitude controlled + const bool vehicle_is_landing_or_descending = _v_control_mode->flag_control_auto_enabled + && ((_attc->get_pos_sp_triplet()->current.valid + && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND) || + (_v_control_mode->flag_control_climb_rate_enabled && !_v_control_mode->flag_control_altitude_enabled)); + // disable pusher assist depending on setting of forward_thrust_enable_mode: switch (_param_vt_fwd_thrust_en.get()) { case DISABLE: // disable in all modes return 0.0f; break; - case ENABLE_WITHOUT_LAND: // disable in land mode - if (_attc->get_pos_sp_triplet()->current.valid - && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND - && _v_control_mode->flag_control_auto_enabled) { + case ENABLE_WITHOUT_LAND: // disable in land/descend mode + if (vehicle_is_landing_or_descending) { return 0.0f; } @@ -464,10 +478,8 @@ float VtolType::pusher_assist() break; - case ENABLE_ABOVE_MPC_LAND_ALT1_WITHOUT_LAND: // disable if below MPC_LAND_ALT1 or in land mode - if ((_attc->get_pos_sp_triplet()->current.valid - && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND - && _v_control_mode->flag_control_auto_enabled) || + case ENABLE_ABOVE_MPC_LAND_ALT1_WITHOUT_LAND: // disable if below MPC_LAND_ALT1 or in land/descend mode + if (vehicle_is_landing_or_descending || (!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _param_mpc_land_alt1.get()))) { return 0.0f; } @@ -475,9 +487,7 @@ float VtolType::pusher_assist() break; case ENABLE_ABOVE_MPC_LAND_ALT2_WITHOUT_LAND: // disable if below MPC_LAND_ALT2 or in land mode - if ((_attc->get_pos_sp_triplet()->current.valid - && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND - && _v_control_mode->flag_control_auto_enabled) || + if (vehicle_is_landing_or_descending || (!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _param_mpc_land_alt2.get()))) { return 0.0f; } @@ -485,10 +495,9 @@ float VtolType::pusher_assist() break; } - // if the thrust scale param is zero or the drone is not in some position or altitude control mode, + // if the thrust scale param is zero or the drone is not in a climb rate controlled mode, // then the pusher-for-pitch strategy is disabled and we can return - if (_param_vt_fwd_thrust_sc.get() < FLT_EPSILON || !(_v_control_mode->flag_control_position_enabled - || _v_control_mode->flag_control_altitude_enabled)) { + if (_param_vt_fwd_thrust_sc.get() < FLT_EPSILON || !(_v_control_mode->flag_control_climb_rate_enabled)) { return 0.0f; } @@ -598,18 +607,19 @@ float VtolType::getOpenLoopFrontTransitionTime() const } float VtolType::getTransitionAirspeed() const { - return math::max(_param_vt_arsp_trans.get(), getMinimumTransitionAirspeed()); -} -float VtolType::getMinimumTransitionAirspeed() const -{ + // Since the stall airspeed increases with vehicle weight, we increase the transition airspeed + // by the same factor. + float weight_ratio = 1.0f; if (_param_weight_base.get() > FLT_EPSILON && _param_weight_gross.get() > FLT_EPSILON) { - weight_ratio = math::constrain(_param_weight_gross.get() / _param_weight_base.get(), kMinWeightRatio, kMaxWeightRatio); + weight_ratio = math::constrain(_param_weight_gross.get() / + _param_weight_base.get(), kMinWeightRatio, kMaxWeightRatio); } - return sqrtf(weight_ratio) * _param_airspeed_min.get(); + return sqrtf(weight_ratio) * _param_vt_arsp_trans.get(); } + float VtolType::getBlendAirspeed() const { return _param_vt_arsp_blend.get(); diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index c8cecb8fb041..d59887372efb 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -237,12 +237,6 @@ class VtolType : public ModuleParams */ float getOpenLoopFrontTransitionTime() const; - /** - * - * @return The minimum calibrated airspeed compensated for weight [m/s] - */ - float getMinimumTransitionAirspeed() const; - /** * * @return The calibrated blending airspeed [m/s] @@ -322,7 +316,7 @@ class VtolType : public ModuleParams hrt_abstime _last_loop_ts = 0; float _transition_dt = 0; - float _quadchute_ref_alt{-MAXFLOAT}; // altitude (AMSL) reference to compute quad-chute altitude loss condition + float _quadchute_ref_alt{NAN}; // altitude (AMSL) reference to compute quad-chute altitude loss condition float _accel_to_pitch_integ = 0; @@ -371,8 +365,7 @@ class VtolType : public ModuleParams (ParamFloat) _param_mpc_land_alt2, (ParamFloat) _param_vt_lnd_pitch_min, (ParamFloat) _param_weight_base, - (ParamFloat) _param_weight_gross, - (ParamFloat) _param_airspeed_min + (ParamFloat) _param_weight_gross ) diff --git a/src/modules/zenoh/Kconfig.topics b/src/modules/zenoh/Kconfig.topics index a1fe848d0738..14866f5ebff6 100644 --- a/src/modules/zenoh/Kconfig.topics +++ b/src/modules/zenoh/Kconfig.topics @@ -65,6 +65,10 @@ menu "Zenoh publishers/subscribers" bool "battery_status" default n + config ZENOH_PUBSUB_BUFFER128 + bool "buffer128" + default n + config ZENOH_PUBSUB_BUTTON_EVENT bool "button_event" default n @@ -129,6 +133,10 @@ menu "Zenoh publishers/subscribers" bool "debug_vect" default n + config ZENOH_PUBSUB_DIFFERENTIAL_DRIVE_SETPOINT + bool "differential_drive_setpoint" + default n + config ZENOH_PUBSUB_DIFFERENTIAL_PRESSURE bool "differential_pressure" default n @@ -217,6 +225,10 @@ menu "Zenoh publishers/subscribers" bool "figure_eight_status" default n + config ZENOH_PUBSUB_FLIGHT_PHASE_ESTIMATION + bool "flight_phase_estimation" + default n + config ZENOH_PUBSUB_FOLLOW_TARGET bool "follow_target" default n @@ -549,6 +561,10 @@ menu "Zenoh publishers/subscribers" bool "sensor_accel_fifo" default n + config ZENOH_PUBSUB_SENSOR_AIRFLOW + bool "sensor_airflow" + default n + config ZENOH_PUBSUB_SENSOR_BARO bool "sensor_baro" default n @@ -789,6 +805,10 @@ menu "Zenoh publishers/subscribers" bool "vehicle_trajectory_waypoint" default n + config ZENOH_PUBSUB_VELOCITY_LIMITS + bool "velocity_limits" + default n + config ZENOH_PUBSUB_VTOL_VEHICLE_STATUS bool "vtol_vehicle_status" default n @@ -826,6 +846,7 @@ config ZENOH_PUBSUB_ALL_SELECTION select ZENOH_PUBSUB_ARMING_CHECK_REQUEST select ZENOH_PUBSUB_AUTOTUNE_ATTITUDE_CONTROL_STATUS select ZENOH_PUBSUB_BATTERY_STATUS + select ZENOH_PUBSUB_BUFFER128 select ZENOH_PUBSUB_BUTTON_EVENT select ZENOH_PUBSUB_CAMERA_CAPTURE select ZENOH_PUBSUB_CAMERA_STATUS @@ -842,6 +863,7 @@ config ZENOH_PUBSUB_ALL_SELECTION select ZENOH_PUBSUB_DEBUG_KEY_VALUE select ZENOH_PUBSUB_DEBUG_VALUE select ZENOH_PUBSUB_DEBUG_VECT + select ZENOH_PUBSUB_DIFFERENTIAL_DRIVE_SETPOINT select ZENOH_PUBSUB_DIFFERENTIAL_PRESSURE select ZENOH_PUBSUB_DISTANCE_SENSOR select ZENOH_PUBSUB_EKF2_TIMESTAMPS @@ -864,6 +886,7 @@ config ZENOH_PUBSUB_ALL_SELECTION select ZENOH_PUBSUB_FAILSAFE_FLAGS select ZENOH_PUBSUB_FAILURE_DETECTOR_STATUS select ZENOH_PUBSUB_FIGURE_EIGHT_STATUS + select ZENOH_PUBSUB_FLIGHT_PHASE_ESTIMATION select ZENOH_PUBSUB_FOLLOW_TARGET select ZENOH_PUBSUB_FOLLOW_TARGET_ESTIMATOR select ZENOH_PUBSUB_FOLLOW_TARGET_STATUS @@ -947,6 +970,7 @@ config ZENOH_PUBSUB_ALL_SELECTION select ZENOH_PUBSUB_SATELLITE_INFO select ZENOH_PUBSUB_SENSOR_ACCEL select ZENOH_PUBSUB_SENSOR_ACCEL_FIFO + select ZENOH_PUBSUB_SENSOR_AIRFLOW select ZENOH_PUBSUB_SENSOR_BARO select ZENOH_PUBSUB_SENSOR_COMBINED select ZENOH_PUBSUB_SENSOR_CORRECTION @@ -1007,6 +1031,7 @@ config ZENOH_PUBSUB_ALL_SELECTION select ZENOH_PUBSUB_VEHICLE_TORQUE_SETPOINT select ZENOH_PUBSUB_VEHICLE_TRAJECTORY_BEZIER select ZENOH_PUBSUB_VEHICLE_TRAJECTORY_WAYPOINT + select ZENOH_PUBSUB_VELOCITY_LIMITS select ZENOH_PUBSUB_VTOL_VEHICLE_STATUS select ZENOH_PUBSUB_WHEEL_ENCODERS select ZENOH_PUBSUB_WIND diff --git a/src/modules/zenoh/subscribers/uorb_subscriber.hpp b/src/modules/zenoh/subscribers/uorb_subscriber.hpp index bd16b64398fd..550c1a3f4584 100644 --- a/src/modules/zenoh/subscribers/uorb_subscriber.hpp +++ b/src/modules/zenoh/subscribers/uorb_subscriber.hpp @@ -55,7 +55,7 @@ class uORB_Zenoh_Subscriber : public Zenoh_Subscriber _cdr_ops(ops) { int instance = 0; - _uorb_pub_handle = orb_advertise_multi_queue(_uorb_meta, nullptr, &instance, 1); //FIXME template magic qsize + _uorb_pub_handle = orb_advertise_multi(_uorb_meta, nullptr, &instance); }; ~uORB_Zenoh_Subscriber() override = default; diff --git a/src/systemcmds/netman/Kconfig b/src/systemcmds/netman/Kconfig index d94b0ff82be3..d2d122339793 100644 --- a/src/systemcmds/netman/Kconfig +++ b/src/systemcmds/netman/Kconfig @@ -10,3 +10,11 @@ menuconfig USER_NETMAN depends on BOARD_PROTECTED && SYSTEMCMDS_NETMAN ---help--- Put netman in userspace memory + +config NETMAN_FALLBACK_IPADDR + hex "Fallback IPv4 address" + default 0XC0A80003 + depends on SYSTEMCMDS_NETMAN + ---help--- + If NETINIT_DHCPC is set, getting an IP from DHCP server is first attempted. + If this fails, this IP address is used as a static fallback. diff --git a/src/systemcmds/netman/netman.cpp b/src/systemcmds/netman/netman.cpp index ec423b8974ad..b9e653bdfeb1 100644 --- a/src/systemcmds/netman/netman.cpp +++ b/src/systemcmds/netman/netman.cpp @@ -53,8 +53,8 @@ constexpr char DEFAULT_NETMAN_CONFIG[] = "/fs/microsd/net.cfg"; #if defined(CONFIG_NETINIT_DHCPC) -# define DEFAULT_PROTO IPv4PROTO_FALLBACK -# define DEFAULT_IP 0XC0A80003 // 192.168.0.3 +# define DEFAULT_PROTO IPv4PROTO_FALLBACK +# define DEFAULT_IP CONFIG_NETMAN_FALLBACK_IPADDR #else # define DEFAULT_PROTO IPv4PROTO_STATIC # define DEFAULT_IP CONFIG_NETINIT_IPADDR @@ -201,7 +201,7 @@ class net_params struct ipv4cfg_s ipcfg; int rv = ipcfg_read(netdev, (FAR struct ipcfg_s *) &ipcfg, AF_INET); - if (rv == -EINVAL || + if (rv == -EINVAL || rv == -ENOENT || (rv == OK && (ipcfg.proto > IPv4PROTO_FALLBACK || ipcfg.ipaddr == 0xffffffff))) { // Build a default ipcfg.ipaddr = HTONL(DEFAULT_IP); @@ -375,7 +375,7 @@ int update(const char *path, const char *netdev) sleep(1); - px4_reboot_request(false); + px4_reboot_request(REBOOT_REQUEST); while (1) { px4_usleep(1); } // this command should not return on success diff --git a/src/systemcmds/reboot/reboot.cpp b/src/systemcmds/reboot/reboot.cpp index 8d0b3419e81b..6e5c159d7008 100644 --- a/src/systemcmds/reboot/reboot.cpp +++ b/src/systemcmds/reboot/reboot.cpp @@ -44,6 +44,7 @@ #include #include #include +#include static void print_usage() { @@ -51,6 +52,9 @@ static void print_usage() PRINT_MODULE_USAGE_NAME_SIMPLE("reboot", "command"); PRINT_MODULE_USAGE_PARAM_FLAG('b', "Reboot into bootloader", true); +#ifdef BOARD_HAS_ISP_BOOTLOADER + PRINT_MODULE_USAGE_PARAM_FLAG('i', "Reboot into ISP (1st stage bootloader)", true); +#endif PRINT_MODULE_USAGE_ARG("lock|unlock", "Take/release the shutdown lock (for testing)", true); } @@ -58,17 +62,24 @@ static void print_usage() extern "C" __EXPORT int reboot_main(int argc, char *argv[]) { int ch; - bool to_bootloader = false; + reboot_request_t request = REBOOT_REQUEST; int myoptind = 1; const char *myoptarg = nullptr; - while ((ch = px4_getopt(argc, argv, "b", &myoptind, &myoptarg)) != -1) { + while ((ch = px4_getopt(argc, argv, "bi", &myoptind, &myoptarg)) != -1) { switch (ch) { case 'b': - to_bootloader = true; + request = REBOOT_TO_BOOTLOADER; break; +#ifdef BOARD_HAS_ISP_BOOTLOADER + + case 'i': + request = REBOOT_TO_ISP; + break; +#endif + default: print_usage(); return 1; @@ -98,7 +109,7 @@ extern "C" __EXPORT int reboot_main(int argc, char *argv[]) return ret; } - int ret = px4_reboot_request(to_bootloader); + int ret = px4_reboot_request(request); if (ret < 0) { PX4_ERR("reboot failed (%i)", ret); diff --git a/src/systemcmds/sd_bench/sd_bench.cpp b/src/systemcmds/sd_bench/sd_bench.cpp index 990932432162..edfaff4f5062 100644 --- a/src/systemcmds/sd_bench/sd_bench.cpp +++ b/src/systemcmds/sd_bench/sd_bench.cpp @@ -50,6 +50,8 @@ #include +#define MAX(a,b) ((a) > (b) ? (a) : (b)) + typedef struct sdb_config { int num_runs; ///< number of runs int run_duration; ///< duration of a single run [ms] @@ -202,6 +204,7 @@ void write_test(int fd, sdb_config_t *cfg, uint8_t *block, int block_size) unsigned int total_blocks = 0; cfg->total_blocks_written = 0; unsigned int *blocknumber = (unsigned int *)(void *)&block[0]; + unsigned int max_max_write_time = 0; for (int run = 0; run < cfg->num_runs; ++run) { hrt_abstime start = hrt_absolute_time(); @@ -245,10 +248,12 @@ void write_test(int fd, sdb_config_t *cfg, uint8_t *block, int block_size) total_elapsed += elapsed; total_blocks += num_blocks; + max_max_write_time = MAX(max_max_write_time, max_write_time); } cfg->total_blocks_written = total_blocks; PX4_INFO(" Avg : %8.2lf KB/s", (double)block_size * total_blocks / total_elapsed / 1024.); + PX4_INFO(" Overall max write time: %i ms", max_max_write_time); } int read_test(int fd, sdb_config_t *cfg, uint8_t *block, int block_size) diff --git a/src/systemcmds/tests/test_dataman.cpp b/src/systemcmds/tests/test_dataman.cpp index f5cbf2554086..bea7a7ca416b 100644 --- a/src/systemcmds/tests/test_dataman.cpp +++ b/src/systemcmds/tests/test_dataman.cpp @@ -153,14 +153,14 @@ DatamanTest::testSyncWriteInvalidItem() bool DatamanTest::testSyncReadInvalidIndex() { - bool success = _dataman_client1.readSync(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, _buffer_read, 2); + bool success = _dataman_client1.readSync(DM_KEY_SAFE_POINTS_0, 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); + bool success = _dataman_client1.writeSync(DM_KEY_SAFE_POINTS_0, DM_KEY_SAFE_POINTS_MAX, _buffer_write, 2); return !success; } @@ -238,7 +238,7 @@ DatamanTest::testSyncWriteReadAllItemsMaxSize() bool success = false; // Iterate all items - for (uint32_t item = DM_KEY_SAFE_POINTS; item < DM_KEY_NUM_KEYS; ++item) { + for (uint32_t item = DM_KEY_SAFE_POINTS_0; item < DM_KEY_NUM_KEYS; ++item) { // writeSync for (uint32_t index = 0U; index < _max_index[item]; ++index) { @@ -289,7 +289,7 @@ DatamanTest::testSyncClearAll() bool success = false; // Iterate all items - for (uint32_t item = DM_KEY_SAFE_POINTS; item < DM_KEY_NUM_KEYS; ++item) { + for (uint32_t item = DM_KEY_SAFE_POINTS_0; item < DM_KEY_NUM_KEYS; ++item) { success = _dataman_client1.clearSync((dm_item_t)item); @@ -433,7 +433,7 @@ DatamanTest::testAsyncReadInvalidIndex() case State::Read: state = State::ReadWait; - success = _dataman_client1.readAsync(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, _buffer_read, 2); + success = _dataman_client1.readAsync(DM_KEY_SAFE_POINTS_0, DM_KEY_SAFE_POINTS_MAX, _buffer_read, 2); if (!success) { return false; @@ -489,7 +489,7 @@ DatamanTest::testAsyncWriteInvalidIndex() case State::Write: state = State::WriteWait; - success = _dataman_client1.writeAsync(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, _buffer_write, 2); + success = _dataman_client1.writeAsync(DM_KEY_SAFE_POINTS_0, DM_KEY_SAFE_POINTS_MAX, _buffer_write, 2); if (!success) { return false; @@ -529,7 +529,7 @@ DatamanTest::testAsyncWriteInvalidIndex() bool DatamanTest::testAsyncReadBufferOverflow() { - bool success = _dataman_client1.readAsync(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, _buffer_read, OVERFLOW_LENGTH); + bool success = _dataman_client1.readAsync(DM_KEY_SAFE_POINTS_0, DM_KEY_SAFE_POINTS_MAX, _buffer_read, OVERFLOW_LENGTH); return !success; } @@ -537,7 +537,8 @@ DatamanTest::testAsyncReadBufferOverflow() bool DatamanTest::testAsyncWriteBufferOverflow() { - bool success = _dataman_client1.writeAsync(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, _buffer_write, OVERFLOW_LENGTH); + bool success = _dataman_client1.writeAsync(DM_KEY_SAFE_POINTS_0, DM_KEY_SAFE_POINTS_MAX, _buffer_write, + OVERFLOW_LENGTH); return !success; } @@ -715,7 +716,7 @@ DatamanTest::testAsyncWriteReadAllItemsMaxSize() bool success = false; State state = State::Write; - uint32_t item = DM_KEY_SAFE_POINTS; + uint32_t item = DM_KEY_SAFE_POINTS_0; uint32_t index = 0U; hrt_abstime start_time = hrt_absolute_time(); @@ -828,7 +829,7 @@ DatamanTest::testAsyncClearAll() State state = State::Clear; hrt_abstime start_time = hrt_absolute_time(); - uint32_t item = DM_KEY_SAFE_POINTS; + uint32_t item = DM_KEY_SAFE_POINTS_0; //While loop represents a task while (state != State::Exit) { @@ -1076,7 +1077,7 @@ DatamanTest::testResetItems() mission_s mission{}; mission.timestamp = hrt_absolute_time(); - mission.dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0; + mission.mission_dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0; mission.count = 0; mission.current_seq = 0; @@ -1098,19 +1099,19 @@ DatamanTest::testResetItems() stats.num_items = 0; stats.opaque_id = 0; - success = _dataman_client1.writeSync(DM_KEY_FENCE_POINTS, 0, reinterpret_cast(&stats), + success = _dataman_client1.writeSync(DM_KEY_FENCE_POINTS_STATE, 0, reinterpret_cast(&stats), sizeof(mission_stats_entry_s)); if (!success) { - PX4_ERR("failed to reset DM_KEY_FENCE_POINTS"); + PX4_ERR("failed to reset DM_KEY_FENCE_POINTS_STATE"); return false; } - success = _dataman_client1.writeSync(DM_KEY_SAFE_POINTS, 0, reinterpret_cast(&stats), + success = _dataman_client1.writeSync(DM_KEY_SAFE_POINTS_STATE, 0, reinterpret_cast(&stats), sizeof(mission_stats_entry_s)); if (!success) { - PX4_ERR("failed to reset DM_KEY_SAFE_POINTS"); + PX4_ERR("failed to reset DM_KEY_SAFE_POINTS_STATE"); return false; } diff --git a/src/systemcmds/ver/ver.cpp b/src/systemcmds/ver/ver.cpp index ab75009d1ec5..a2b5c1885a5c 100644 --- a/src/systemcmds/ver/ver.cpp +++ b/src/systemcmds/ver/ver.cpp @@ -51,6 +51,9 @@ static const char sz_ver_hw_str[] = "hw"; static const char sz_ver_hwcmp_str[] = "hwcmp"; static const char sz_ver_hwtypecmp_str[] = "hwtypecmp"; +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) +static const char sz_ver_hwbasecmp_str[] = "hwbasecmp"; +#endif static const char sz_ver_git_str[] = "git"; static const char sz_ver_bdate_str[] = "bdate"; static const char sz_ver_buri_str[] = "uri"; @@ -84,6 +87,11 @@ static void usage(const char *reason) PRINT_MODULE_USAGE_COMMAND_DESCR("hwtypecmp", "Compare hardware type (returns 0 on match)"); PRINT_MODULE_USAGE_ARG(" []", "Hardware type to compare against (eg. V2). An OR comparison is used if multiple are specified", false); +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) + PRINT_MODULE_USAGE_COMMAND_DESCR("hwbasecmp", "Compare hardware base (returns 0 on match)"); + PRINT_MODULE_USAGE_ARG(" []", + "Hardware type to compare against (eg. V2). An OR comparison is used if multiple are specified", false); +#endif } extern "C" __EXPORT int ver_main(int argc, char *argv[]) @@ -129,12 +137,50 @@ extern "C" __EXPORT int ver_main(int argc, char *argv[]) return 1; } +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) + + if (!strncmp(argv[1], sz_ver_hwbasecmp_str, sizeof(sz_ver_hwbasecmp_str))) { + if (argc >= 3 && argv[2] != nullptr) { + const char *board_type = px4_board_base_type(); + + for (int i = 2; i < argc; ++i) { + if (strcmp(board_type, argv[i]) == 0) { + return 0; // if one of the arguments match, return success + } + } + + } else { + PX4_ERR("Not enough arguments, try 'ver hwbasecmp {000...999}[1:*]'"); + } + + return 1; + } + +#endif /* check if we want to show all */ bool show_all = !strncmp(argv[1], sz_ver_all_str, sizeof(sz_ver_all_str)); if (show_all || !strncmp(argv[1], sz_ver_hw_str, sizeof(sz_ver_hw_str))) { PX4_INFO_RAW("HW arch: %s\n", px4_board_name()); -#if defined(BOARD_HAS_VERSIONING) + +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) + char sbase[14] = "NA"; + char sfmum[14] = "NA"; + int base = GET_HW_BASE_ID(); + int fmu = GET_HW_FMUM_ID(); + + if (base >= 0) { + snprintf(sbase, sizeof(sbase), "0x%0" STRINGIFY(HW_INFO_VER_DIGITS) "X", base); + } + + if (fmu >= 0) { + snprintf(sfmum, sizeof(sfmum), "0x%0" STRINGIFY(HW_INFO_REV_DIGITS) "X", fmu); + } + + PX4_INFO_RAW("HW type: %s\n", strlen(HW_INFO_INIT_PREFIX) ? HW_INFO_INIT_PREFIX : "NA"); + PX4_INFO_RAW("HW FMUM ID: %s\n", sfmum); + PX4_INFO_RAW("HW BASE ID: %s\n", sbase); +#elif defined(BOARD_HAS_VERSIONING) char vb[14] = "NA"; char rb[14] = "NA"; int v = px4_board_hw_version(); diff --git a/test/mavsdk_tests/autopilot_tester.cpp b/test/mavsdk_tests/autopilot_tester.cpp index 0c8e774c41f7..e1bbe9efde85 100644 --- a/test/mavsdk_tests/autopilot_tester.cpp +++ b/test/mavsdk_tests/autopilot_tester.cpp @@ -438,15 +438,18 @@ void AutopilotTester::fly_forward_in_posctl() } CHECK(_manual_control->start_position_control() == ManualControl::Result::Success); + store_home(); + wait_until_ready(); + arm(); - // Climb up for 20 seconds - for (unsigned i = 0; i < 20 * manual_control_rate_hz; ++i) { + // Climb up for 5 seconds + for (unsigned i = 0; i < 5 * manual_control_rate_hz; ++i) { CHECK(_manual_control->set_manual_control_input(0.f, 0.f, 1.f, 0.f) == ManualControl::Result::Success); sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz)); } - // Fly forward for 60 seconds - for (unsigned i = 0; i < 60 * manual_control_rate_hz; ++i) { + // Fly forward for 10 seconds + for (unsigned i = 0; i < 10 * manual_control_rate_hz; ++i) { CHECK(_manual_control->set_manual_control_input(0.5f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success); sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz)); } @@ -473,15 +476,18 @@ void AutopilotTester::fly_forward_in_altctl() } CHECK(_manual_control->start_altitude_control() == ManualControl::Result::Success); + store_home(); + wait_until_ready(); + arm(); - // Climb up for 20 seconds - for (unsigned i = 0; i < 20 * manual_control_rate_hz; ++i) { + // Climb up for 5 seconds + for (unsigned i = 0; i < 5 * manual_control_rate_hz; ++i) { CHECK(_manual_control->set_manual_control_input(0.f, 0.f, 1.f, 0.f) == ManualControl::Result::Success); sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz)); } - // Fly forward for 60 seconds - for (unsigned i = 0; i < 60 * manual_control_rate_hz; ++i) { + // Fly forward for 10 seconds + for (unsigned i = 0; i < 10 * manual_control_rate_hz; ++i) { CHECK(_manual_control->set_manual_control_input(0.5f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success); sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz)); } diff --git a/test/mavsdk_tests/test_multicopter_manual.cpp b/test/mavsdk_tests/test_multicopter_manual.cpp index 951f0f241653..34fa8b435167 100644 --- a/test/mavsdk_tests/test_multicopter_manual.cpp +++ b/test/mavsdk_tests/test_multicopter_manual.cpp @@ -38,9 +38,6 @@ TEST_CASE("Fly forward in position control", "[multicopter][vtol]") { AutopilotTester tester; tester.connect(connection_url); - tester.wait_until_ready(); - tester.store_home(); - tester.arm(); tester.fly_forward_in_posctl(); std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(180); tester.wait_until_disarmed(until_disarmed_timeout); @@ -51,9 +48,6 @@ TEST_CASE("Fly forward in altitude control", "[multicopter][vtol]") { AutopilotTester tester; tester.connect(connection_url); - tester.wait_until_ready(); - tester.store_home(); - tester.arm(); tester.fly_forward_in_altctl(); std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(180); tester.wait_until_disarmed(until_disarmed_timeout); diff --git a/test/mavsdk_tests/test_multicopter_offboard.cpp b/test/mavsdk_tests/test_multicopter_offboard.cpp index 2a166e3612b4..7160c0db382c 100644 --- a/test/mavsdk_tests/test_multicopter_offboard.cpp +++ b/test/mavsdk_tests/test_multicopter_offboard.cpp @@ -34,6 +34,7 @@ #include "autopilot_tester.h" #include +static constexpr float acceptance_radius = 0.3f; TEST_CASE("Offboard takeoff and land", "[multicopter][offboard]") { @@ -45,7 +46,7 @@ TEST_CASE("Offboard takeoff and land", "[multicopter][offboard]") tester.set_rc_loss_exception(AutopilotTester::RcLossException::Offboard); tester.arm(); std::chrono::seconds goto_timeout = std::chrono::seconds(90); - tester.offboard_goto(takeoff_position, 0.1f, goto_timeout); + tester.offboard_goto(takeoff_position, acceptance_radius, goto_timeout); tester.offboard_land(); tester.wait_until_disarmed(std::chrono::seconds(120)); tester.check_home_within(1.0f); @@ -63,12 +64,12 @@ TEST_CASE("Offboard position control", "[multicopter][offboard]") tester.store_home(); tester.set_rc_loss_exception(AutopilotTester::RcLossException::Offboard); tester.arm(); - std::chrono::seconds goto_timeout = std::chrono::seconds(120); - tester.offboard_goto(takeoff_position, 0.1f, goto_timeout); - tester.offboard_goto(setpoint_1, 0.1f, goto_timeout); - tester.offboard_goto(setpoint_2, 0.1f, goto_timeout); - tester.offboard_goto(setpoint_3, 0.1f, goto_timeout); - tester.offboard_goto(takeoff_position, 0.1f, goto_timeout); + std::chrono::seconds goto_timeout = std::chrono::seconds(10); + tester.offboard_goto(takeoff_position, acceptance_radius, goto_timeout); + tester.offboard_goto(setpoint_1, acceptance_radius, goto_timeout); + tester.offboard_goto(setpoint_2, acceptance_radius, goto_timeout); + tester.offboard_goto(setpoint_3, acceptance_radius, goto_timeout); + tester.offboard_goto(takeoff_position, acceptance_radius, goto_timeout); tester.offboard_land(); tester.wait_until_disarmed(std::chrono::seconds(120)); tester.check_home_within(1.0f);